├── .gitignore ├── LICENSE ├── README.md ├── cooleye_d1.code-workspace ├── sdk ├── CMakeLists.txt ├── config │ ├── cecfg_std.txt │ ├── cooleye_cam_set.sh │ ├── extrinsics.yml │ ├── intrinsics.yml │ ├── serial.txt │ ├── stereo_calib.xml │ └── updata_rosfile.sh ├── dso_config │ ├── camera.txt │ └── pcalib.txt ├── include │ ├── cedriver_cam.h │ ├── cedriver_config.h │ ├── cedriver_global_config.h │ ├── cedriver_imu.h │ ├── cedriver_usb.h │ ├── celib_MadgwickAHRS.h │ ├── celib_img_process.h │ ├── ceros_cam_d1_driver.h │ ├── cpu_set.h │ ├── logmsg.h │ └── threadsafe_queue.h ├── rosfile │ ├── CMakeLists.txt │ └── package.xml └── src │ ├── ceapp_disp_filter.cpp │ ├── ceapp_raw_data.cpp │ ├── ceapp_stereo_match.cpp │ ├── ceapp_write_deviceid.cpp │ ├── cedriver_cam.cpp │ ├── cedriver_config.cpp │ ├── cedriver_imu.cpp │ ├── cedriver_usb.cpp │ ├── celib_MadgwickAHRS.cpp │ ├── celib_img_process.cpp │ ├── ceros_cam_d1_driver.cpp │ ├── ceros_cam_d1_node.cpp │ ├── cetool_cali_imu.cpp │ ├── cetool_cali_stereo_cal.cpp │ ├── cetool_cali_stereo_capture_img.cpp │ ├── cpu_set.cpp │ └── logmsg.cpp └── tools ├── 1_usb_driver_for_win └── camD1_driver.rar ├── 2_cali_file_for_kalibr ├── checkerboard_11x8_3x3cm.yaml └── imu0_icm20689.yaml ├── 3_updataTool_for_imu └── FlyMcu.zip └── 4_updataTool_for_camera └── release.zip /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | *.swo 3 | *.kdev4 4 | sdk/build/* 5 | sdk/.kdev4 6 | ros/build/* 7 | ros/devel/* 8 | *.catkin_workspace 9 | ros/* 10 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CoolEye D1 Linux SDK & ROS 2 | ### Version 0.4 3 | 本项目包括CoolEye D1 相机运行所需的必备文件,为了便于用于学习和使用,内容全部开源。 4 | 主流的SLAM算法,图像拼接,智能识别等功能会陆续放出。 5 | 6 | 淘宝地址: https://item.taobao.com/item.htm?spm=a1z10.3-c.w4023-18512428132.2.b68367c2Q9BWIH&id=572104551076 7 | 8 | 第三批已到货，欢迎加入。 9 | 10 | 应大家要求组建QQ交流群 11 | 12 | ![enter image description here](https://ws4.sinaimg.cn/large/0069RVTdgy1fv0qsffh1lj306a08274f.jpg) 13 | 14 | ------------------------------------------------------------------------------------------------- 15 | ## 主要参数介绍 16 | 17 | - CMOS 1/3'' : 灰度图像(Mono) 18 | - 快门模式 : 全局快门(global shutter) 19 | - 同步方式 : 双目同步曝光 20 | - 分辨率 : 最大752x480 21 | - 帧率 : 双目45帧 22 | - 视场角 : D/H/V = 140/120/75 23 | - 双目基线长 : 120mm 24 | - 输出格式 : RAW/畸变矫正图/视差图/XYZ点云图 25 | - 深度范围 : 0.2m - 10米左右 26 | - IMU输出 : ACC / GYRO 原始数据,帧率最大1K 27 | - 接口类型 : USB2.0 28 | 29 | 30 | -------------------------------------------------------------------------------------------------- 31 | ## 目录 32 | - 一. SDK & ROS安装 33 | - 1.1 SDK安装方法 34 | - 1.2 ROS环境安装方法 35 | - 二. 标定相机相关 36 | - 2.1 ROS环境下标定相机 37 | - 2.2 kalibr 标定相关 38 | - 三. 运行算法相关 39 | - 3.1 ORB-SLAM2算法的安装和使用 40 | - 3.2 okvis算法的安装和使用 41 | - 3.2 vins算法的安装和使用 42 | - 四. 相机固件升级方法 43 | - 4.1 IMU操作固件升级 44 | - 4.2 图像处理固件升级 45 | - 五. 常见问题汇总 46 | - 5.1 串口无法打开 47 | - 5.2 ORB_SLAM2的ROS环境编译不通过 48 | - 六. 硬件尺寸图 49 | - 修订历史 50 | 51 | ----------------------------------------------------------- 52 | 53 | ![enter image description here](https://ws1.sinaimg.cn/large/006tKfTcgy1ftnkfhworqj30ku0bqmy0.jpg) 54 | 55 | ![enter image description here](https://ws4.sinaimg.cn/large/006tKfTcgy1ftnkggoo84j30ku0bqwfg.jpg) 56 | 57 | ![enter image description here](https://ws1.sinaimg.cn/large/006tKfTcgy1ftnkgp0ekuj30ku0bqwf3.jpg) 58 | 59 | ![enter image description here](https://ws1.sinaimg.cn/large/006tKfTcgy1ftnkh0ehc3j30ku0bq3zr.jpg) 60 | 61 | ![enter image description here](https://ws1.sinaimg.cn/large/006tKfTcgy1ftnkh59ynwj30ku0bqgmp.jpg) 62 | 63 | ![enter image description here](https://ws1.sinaimg.cn/large/006tKfTcgy1ftnkh9x834j30ku0bq0tk.jpg) 64 | 65 | ![enter image description here](https://ws1.sinaimg.cn/large/006tKfTcgy1ftnkhe2b19j30ku0bq751.jpg) 66 | 67 | ![enter image description here](https://ws2.sinaimg.cn/large/006tKfTcgy1ftnkhnj6ipj30ku0bqaaw.jpg) 68 | 69 | ![enter image description here](https://ws2.sinaimg.cn/large/006tKfTcgy1ftnkhv9hpxj30ku0bq75c.jpg) 70 | 71 | -------------------------------------------------------------------------------------------------------- 72 | VINS-Mono 测试效果图 73 | 74 | ![enter image description here](https://ws4.sinaimg.cn/large/006tNc79gy1ftqy85tb83g31hc0u0x6q.gif) 75 | 76 | ![enter image description here](https://ws4.sinaimg.cn/large/006tNc79gy1ftqy8zocseg31hc0u07wk.gif) 77 | 78 | ![enter image description here](https://ws2.sinaimg.cn/large/006tNc79gy1ftqy9h2149g31hc0u0qv7.gif) 79 | 80 | 81 | ------------------------------------------------------------------------------------------------- 82 | ORB_SLAM2 测试效果图 83 | 84 | ![enter image description here](https://ws4.sinaimg.cn/large/0069RVTdgy1fupm9ysjmwg31hc0u0npd.gif) 85 | 86 | ![enter image description here](https://ws3.sinaimg.cn/large/0069RVTdgy1fupmacv01pg31hc0u07wh.gif) 87 | 88 | ![enter image description here](https://ws2.sinaimg.cn/large/0069RVTdgy1fupmakbo79g31hc0u0hdt.gif) 89 | 90 | ![enter image description here](https://ws3.sinaimg.cn/large/0069RVTdgy1fupmastlcpg31hc0u0qv5.gif) 91 | 92 | --------------------------------------------------------------------------------------------------- 93 | 94 | ## 一. SDK & ROS安装 95 | - 推荐环境：ubuntu 16.04 LTS 96 | 97 | - 推荐opencv版本：3.4.0 98 | 99 | - 推荐ROS版本：Kinetic 100 | 101 | 102 | __首先需要安装opencv__ 103 | 建议使用3.4.0版本。 104 | 参考文档： 105 | ``` 106 | https://blog.csdn.net/u013066730/article/details/79411767 107 | ``` 108 | 另视差图的计算,用到了opencv得扩展库. 109 | 请安装opencv_contrib,推荐使用3.4.0(与opencv同版本). 110 | 111 | ``` 112 | cd ~/src 113 | git clone https://github.com/opencv/opencv_contrib/tree/3.4.0 114 | ``` 115 | 建议进入opencv_contrib目录,将需要用到cuda的库文件直接删除,以便编译顺利. 116 | ``` 117 | cd ~/src/opencv_contrib-3.4.0/modules 118 | rm -rf xfeatures2d 119 | ``` 120 | 进入opencv编译目录,重新执行cmake并编译. 121 | _/modules__ 建议使用绝对路径, 122 | ``` 123 | $ cd 124 | $ cmake -DOPENCV_EXTRA_MODULES_PATH=/modules 125 | $ make 126 | $ sudo make install 127 | ``` 128 | 至此,opencv和opencv_contrib安装完毕. 129 | 130 | ### 1.1 SDK安装方法 131 | 为了便于用户一次成功，介绍时将使用绝对使用路径。 132 | ``` 133 | sudo apt-get install libboost-dev libusb-1.0-0-dev libusb-dev git cmake 134 | 135 | mkdir -p ~/src 136 | 137 | cd ~/src 138 | 139 | git clone https://github.com/cooleyecam/cooleye_d1_linux_sdk_ros.git 140 | 141 | cd ~/src/cooleye_d1_linux_sdk_ros/sdk/config 142 | 143 | sudo ./cooleye_cam_set.sh 144 | 145 | mkdir -p ~/src/cooleye_d1_linux_sdk_ros/sdk/build 146 | 147 | cd ~/src/cooleye_d1_linux_sdk_ros/sdk/build 148 | 149 | cmake .. 150 | 151 | make 152 | 153 | ``` 154 | 155 | 编译将生成6个文件： 156 | - CEAPP_RAW_DATA : 运行即可直接显示相机输出。如果log没有IMU数据,说明串口没有正常打开,如果没有看到图像,很有可能是OPENCV没有正确安装. 157 | ``` 158 | ./CEAPP_RAW_DATA 159 | ``` 160 | 161 | 162 | - CETOOL_CALI_IMU : 用于校准相机自带的IMU的初始偏移。这个六面矫正法移植自PX4的飞控固件，用起来比较麻烦，但是效果还可以，在发布自动矫正的软件版本之前，先临时用来修正IMU的误差。 163 | 运行后根据操作，程序会自己将计算出的误差写入配置文件中，用户无需操作。 164 | ``` 165 | ./CETOOL_CALI_IMU 166 | ``` 167 | 168 | 169 | 170 | - CETOOL_STEREO_CAPTURE_IMG : 由于本相机使用得镜头视角很大,使用opencv自带得棋盘格标定并不容易得到正确得参数.因此需要使用鱼眼(fisheye)标定,本程序即为标定采集所需图像得程序,运行后用标定版对着摄像头,即可看到棋盘格是否正确识别,每敲一个回车,可保存一帧当前图像, 171 | ``` 172 | ./CETOOL_STEREO_CAPTURE_IMG 173 | ``` 174 | 175 | 176 | 177 | - CETOOL_CALI_STEREO_CAL : 采集完包含棋盘格得图像后,可调用本程序进行计算.内参和外参文件会直接放入config目录下. 178 | ``` 179 | ./CETOOL_STEREO_CAPTURE_IMG 180 | ``` 181 | 182 | 183 | - CEAPP_STEREO_MATCH : 运行即可调用config目录下的相机内参和外参,并输出视差图, 此程序输出得视差图没有进行后滤波处理.用于调试BM和SGM得算法参数. 184 | ``` 185 | ./CEAPP_STEREO_MATCH 186 | ``` 187 | 188 | 189 | - CEAPP_DISP_FILTER : 运行即可输出经过后滤波得视差图.因为滤波算法尚未整合到opencv,因此需要安装扩展包opencv_contrib,如果安装不正确,则此程序无法正常编译. 本程序同要需要调用config目录下的相机内参和外参. 190 | ``` 191 | ./CEAPP_DISP_FILTER 192 | ``` 193 | 194 | 195 | ### 1.2 ROS环境安装方法 196 | 在这之前请先根据ros官网安装kinetic版本,建议使用kinetic版本.其他的版本在后续使用算法时可能会有些兼容问题.请先安装完 197 | SDK再安装ROS. 198 | 199 | ROS安装参考 200 | ``` 201 | http://wiki.ros.org/kinetic/Installation/Ubuntu 202 | ``` 203 | 搭建好ROS的环境侯，即可运行相机的ROS驱动 204 | ``` 205 | cd ~/src/cooleye_d1_linux_sdk_ros/sdk/config/ 206 | 207 | ./updata_rosfile.sh 208 | 209 | cd ~/src/cooleye_d1_linux_sdk_ros/ros 210 | 211 | catkin_make 212 | 213 | source ~/src/cooleye_d1_linux_sdk_ros/ros/devel/setup.bash 214 | ``` 215 | 216 | 至此编译完成.执行以下命令即可运行相机. 217 | 218 | 219 | ``` 220 | roscore 221 | 222 | rosrun cooleye_d1 CEROS_CAM_D1_NODE ~/src/cooleye_d1_linux_sdk_ros/ros/src/cooleye_d1/config/cecfg_std.txt 223 | ``` 224 | 可使用ros命令观察相机运行状态 225 | ``` 226 | rostopic hz /imu0_icm20689 /cooleyed1/left/image_raw /cooleyed1/right/image_raw 227 | ``` 228 | 229 | 230 | ------------------------------------------------------------------------------------------------- 231 | 232 | ## 二. 标定相机相关 233 | ### 2.1 ROS环境下标定相机 234 | 首先,可以直接参考官方文档,下文的说明只是对官方文档的注解 235 | ``` 236 | http://wiki.ros.org/camera_calibration/Tutorials/StereoCalibration 237 | ``` 238 | 239 | #### 2.1.1 准备工作 240 | 1. 本相机的所有例成均使用3cm正方形的11x8棋盘格,这里的11x8是指内角的数量,其实由12x9个正方向组成. 241 | 2. 光线良好. 242 | 3. 确保CoolEye相机的ROS驱动已正确安装 243 | #### 2.1.2 编译 244 | 安装所需要的ros文件 245 | ``` 246 | rosdep install camera_calibration 247 | 248 | rosmake camera_calibration 249 | ``` 250 | #### 2.1.3 使用cooleye相机在ros环境下发布左右图像的topic 251 | ``` 252 | roscore 253 | 254 | rosrun cooleye_d1 CEROS_CAM_D1_NODE ~/src/cooleye_d1_linux_sdk_ros/ros/src/cooleye_d1/config/cecfg_std.txt 255 | ``` 256 | 可使用ros命令观察相机运行状态 257 | ``` 258 | rostopic hz /imu0_icm20689 /cooleyed1/left/image_raw /cooleyed1/right/image_raw 259 | ``` 260 | #### 2.1.4 启动校准 261 | ``` 262 | rosrun camera_calibration cameracalibrator.py --size 11x8 \ 263 | --square 0.03 --no-service-check --approximate=0.1 \ 264 | right:=/cooleyed1/right/image_raw \ 265 | left:=/cooleyed1/left/image_raw \ 266 | right_camera:=/cooleyed1/right \ 267 | left_camera:=/cooleyed1/left 268 | ``` 269 | 270 | #### 2.1.5 移动棋盘格 271 | 注意事项: 272 | 1. 水平放置棋盘格,即水平方向格子数多余垂直方向. 273 | 2. 视野的左右边缘检测棋盘 274 | 3. 视野的顶部和底部检测棋盘 275 | 4. 棋盘以各种角度朝向摄像头,尽量激活XYZ三轴. 276 | 5. 棋盘格子充满整个视野. 277 | 当收集的数据足够时,CALIBRATE按钮会亮起来,按下,1分钟左右即可获得相机内参. 278 | 标定的结果像素误差小于0.25认为是可以接受的,小于0.1认为是极好的. 279 | 280 | 相机的参考标定结果如下: 281 | ``` 282 | Left: 283 | ('D = ', [-0.2816607211257644, 0.05884233698719249, 0.000989828550094559, -0.002972305970125211, 0.0]) 284 | ('K = ', [370.79682914159343, 0.0, 402.25597436269385, 0.0, 371.44796549024153, 237.55089778032217, 0.0, 0.0, 1.0]) 285 | ('R = ', [0.9980214136742988, -0.007980214436348127, 0.06236644951497698, 0.00797293200390816, 0.9999681488257253, 0.0003656354924037826, -0.062367380919959504, 0.00013233141028975813, 0.9980532512273995]) 286 | ('P = ', [329.35169351670595, 0.0, 357.3512191772461, 0.0, 0.0, 329.35169351670595, 236.6960849761963, 0.0, 0.0, 0.0, 1.0, 0.0]) 287 | 288 | Right: 289 | ('D = ', [-0.2933299350827631, 0.06663661662397566, 0.0015052397322707109, 0.0006085899317121811, 0.0]) 290 | ('K = ', [371.81317417765734, 0.0, 362.62919659065994, 0.0, 371.7958559505307, 230.5395237103048, 0.0, 0.0, 1.0]) 291 | ('R = ', [0.9999953742094383, -0.002431354273887554, -0.001827587513735629, 0.002431140959735301, 0.9999970377000802, -0.00011893148697697306, 0.0018278712644524478, 0.00011448781396267668, 0.9999983228881842]) 292 | ('P = ', [329.35169351670595, 0.0, 357.3512191772461, -39.191260271159535, 0.0, 329.35169351670595, 236.6960849761963, 0.0, 0.0, 0.0, 1.0, 0.0]) 293 | ('self.T ', [-0.11899461806960394, 0.00028931941154418633, 0.00021747408417511582]) 294 | ('self.R ', [0.9979221808144039, -0.005548872111724627, 0.06419136279058048, 0.00553922445102019, 0.9999846044989682, 0.0003282644106577577, -0.06419219602962196, 2.7988029746548842e-05, 0.9979375537505164]) 295 | # oST version 5.0 parameters 296 | 297 | 298 | [image] 299 | 300 | width 301 | 752 302 | 303 | height 304 | 480 305 | 306 | [narrow_stereo/left] 307 | 308 | camera matrix 309 | 370.796829 0.000000 402.255974 310 | 0.000000 371.447965 237.550898 311 | 0.000000 0.000000 1.000000 312 | 313 | distortion 314 | -0.281661 0.058842 0.000990 -0.002972 0.000000 315 | 316 | rectification 317 | 0.998021 -0.007980 0.062366 318 | 0.007973 0.999968 0.000366 319 | -0.062367 0.000132 0.998053 320 | 321 | projection 322 | 329.351694 0.000000 357.351219 0.000000 323 | 0.000000 329.351694 236.696085 0.000000 324 | 0.000000 0.000000 1.000000 0.000000 325 | 326 | # oST version 5.0 parameters 327 | 328 | 329 | [image] 330 | 331 | width 332 | 752 333 | 334 | height 335 | 480 336 | 337 | [narrow_stereo/right] 338 | 339 | camera matrix 340 | 371.813174 0.000000 362.629197 341 | 0.000000 371.795856 230.539524 342 | 0.000000 0.000000 1.000000 343 | 344 | distortion 345 | -0.293330 0.066637 0.001505 0.000609 0.000000 346 | 347 | rectification 348 | 0.999995 -0.002431 -0.001828 349 | 0.002431 0.999997 -0.000119 350 | 0.001828 0.000114 0.999998 351 | 352 | projection 353 | 329.351694 0.000000 357.351219 -39.191260 354 | 0.000000 329.351694 236.696085 0.000000 355 | 0.000000 0.000000 1.000000 0.000000 356 | 357 | ``` 358 | 359 | 360 | 361 | ### 2.2 kalibr 标定相关 362 | kalibr是一款常用的标定工具，可完成camera、imu等标定，很多人喜欢使用。 363 | #### 2.2.1 kalibr安装 364 | 请先安装完ROS 365 | 366 | 1. 安装编译和依赖库 367 | 368 | ``` 369 | sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev 370 | 371 | sudo pip install python-igraph --upgrade 372 | ``` 373 | 374 | 2. 建立catkin工作空间 375 | 376 | ``` 377 | mkdir -p ~/kalibr_workspace/src 378 | 379 | cd ~/kalibr_workspace 380 | 381 | source /opt/ros/kinetic/setup.bash 382 | 383 | catkin init 384 | 385 | catkin config --extend /opt/ros/kinetic 386 | 387 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 388 | ``` 389 | 390 | 3. 下载源代码至src文件目录 391 | ``` 392 | cd ~/kalibr_workspace/src 393 | 394 | git clone https://github.com/ethz-asl/Kalibr.git 395 | ``` 396 | 397 | 4. 编译项目。__-j4__ 根据自己电脑选择，四核就用 __-j4__ ，8核就用 __-j8__ 。编译需要很久，很久，真的很久，很久。 398 | ``` 399 | cd ~/kalibr_workspace 400 | 401 | catkin build -DCMAKE_BUILD_TYPE=Release -j4 402 | ``` 403 | 404 | 5. 添加工作空间的设置 405 | ``` 406 | source ~/kalibr_workspace/devel/setup.bash 407 | ``` 408 | #### 2.2.2 kalibr常用命令简介 409 | ##### kalibr_calibrate_cameras 410 | kalibr_calibrate_cameras 可直接标定双目或多目相机。 411 | ``` 412 | kalibr_calibrate_cameras --target april_6x6.yaml \ 413 | --bag static.bag \ 414 | --models pinhole-equi pinhole-equi \ 415 | --topics /cam0/image_raw /cam1/image_raw 416 | ``` 417 | - --target checkerboard_11x8_3x3cm.yaml 418 | 标定时使用的标定板描述文件，demo使用的是11x8的3cm棋盘格。 419 | 420 | - --bag static.bag 421 | 使用rosbag record 命令，可采集并记录.bag文件，调用采集的图像即可进行标定。 422 | 423 | - --models pinhole-equi pinhole-equi 424 | 选择不同的的相机模型，CoolEye D1 的镜头视角特别大，用鱼眼模型标定效果会好很多”omni-radtan“。 425 | 426 | - --topics /camera/left/image_raw /camera/right/image_raw 427 | bag里记录的topic名称。 428 | 429 | ##### kalibr_calibrate_imu_camera 430 | kalibr_calibrate_imu_camera 可标定imu和相机之间的参数。 431 | ``` 432 | kalibr_calibrate_imu_camera --bag imu2cam.bag \ 433 | --cam camchain-2018-05-06-21-35-46.yaml \ 434 | --imu imu0_icm20689.yaml \ 435 | --target checkerboard_11x8_3x3cm.yaml \ 436 | --time-calibration 437 | ``` 438 | - --bag imu2cam.bag 439 | 使用rosbag record 命令，可采集并记录.bag文件，调用采集的图像即可进行标定。记录imu和相机的时候，保持标定板不动，移动相机，尽量激活所有的imu维度。 440 | 441 | - --cam camchain-2018-05-06-21-35-46.yaml 442 | 带imu的相机标定需要先标定好相机的内参，这里输入相机的内参文件。 443 | 444 | - --target checkerboard_11x8_3x3cm.yaml 445 | 标定时使用的标定板描述文件，demo使用的是11x8的3cm棋盘格。 446 | 447 | - --imu imu-mpu6050.yaml 448 | imu的参数需要通过yaml文件输入给算法。 449 | 450 | 451 | #### 2.2.3 kalibr标定demo 452 | 首先,ros环境下运行起相机 453 | ``` 454 | roscore 455 | 456 | rosrun cooleye_d1 CEROS_CAM_D1_NODE ~/src/cooleye_d1_linux_sdk_ros/ros/src/cooleye_d1/config/cecfg_std.txt 457 | ``` 458 | 可使用ros命令观察相机运行状态 459 | ``` 460 | rostopic hz /imu0_icm20689 /cooleyed1/left/image_raw /cooleyed1/right/image_raw 461 | ``` 462 | 463 | config/cecfg_std.txt 文件中的cf_ros_showimage参数可控制ROS是否显示图像.如果需要显示图像, 将该参数置1即可.如不需要显示,可设置为0.默认设置为1. 464 | ``` 465 | cf_ros_showimage=1; 466 | ``` 467 | 运行ROS命令,采集一些图像数据. 468 | ``` 469 | rosbag record /cooleyed1/left/image_raw /cooleyed1/right/image_raw 470 | ``` 471 | 运行record命令后,系统即开始记录采集到的图像,这时可以拿出标定板对着镜头晃了,规则与ROS下标定相同.参考章节 __2.1.5 移动棋盘格__ 472 | 473 | 然后就可以启动kalibr命令进行标定了 474 | ``` 475 | kalibr_calibrate_cameras --models pinhole-radtan pinhole-radtan \ 476 | --target checkerboard_11x8_3x3cm.yaml\ 477 | --bag Num-18040301-2018-05-06-21-21-34.bag \ 478 | --topics /cooleyed1/left/image_raw /cooleyed1/right/image_raw 479 | ``` 480 | 等一会,程序运行完成后即可获得标定结果. 481 | 482 | 如果还需要继续标定imu.可在完成相机标定后,使用如下命令,同时记录IMU数据和图像数据. 483 | 记录时,保持标定板静止,移动相机,尽量激活IMU的XYZ3个轴. 484 | 将命令中的文件替换为自己的文件名称即可. 485 | 运行算法后可的到IMU与相机的标定结果. 486 | ``` 487 | rosbag record /imu0_icm20689 /camera/left/image_raw /camera/right/image_raw 488 | 489 | kalibr_calibrate_imu_camera --bag 2018-05-09-23-55-02.bag \ 490 | --cam camchain-2018-05-06-21-35-46.yaml \ 491 | --imu imu0_icm20689.yaml \ 492 | --target checkerboard_11x8_3x3cm.yaml \ 493 | --time-calibration 494 | ``` 495 | 496 | 497 | ------------------------------------------------------------------------------------------------- 498 | 499 | ## 三. 运行算法相关 500 | 501 | ### 3.1 ORB-SLAM2算法的安装和使用 502 | ORB_SLAM2是比较火的算法，并熟悉SLAM的人也可以通过它快速搭建SLAM算法的环境。 503 | 这个算法依赖的包比较多，说不准哪天就更新一下，建议还是follow官方的Readme安装，网上也有很多指导教程。 504 | 505 | #### 安装Pangolin 506 | 安装依赖包 507 | ``` 508 | sudo apt-get install libglew-dev 509 | 510 | sudo apt-get install libpython2.7-dev 511 | 512 | sudo apt-get install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev libavdevice-dev 513 | 514 | sudo apt-get install libdc1394-22-dev libraw1394-dev 515 | 516 | sudo apt-get install libjpeg-dev libpng12-dev libtiff5-dev libopenexr-dev 517 | ``` 518 | 安装一个libuvc 519 | ``` 520 | mkdir -p ~/src 521 | 522 | cd ~/src 523 | 524 | git clone https://github.com/ktossell/libuvc 525 | 526 | cd libuvc 527 | 528 | mkdir build 529 | 530 | cd build 531 | 532 | cmake .. 533 | 534 | make && sudo make install 535 | ``` 536 | 下载 Pangolin 537 | ``` 538 | mkdir -p ~/src 539 | 540 | cd ~/src 541 | 542 | git clone https://github.com/stevenlovegrove/Pangolin.git 543 | 544 | cd Pangolin 545 | 546 | mkdir build 547 | 548 | cd build 549 | 550 | cmake .. 551 | 552 | cmake --build . 553 | ``` 554 | Pangolin python bindings 555 | ``` 556 | sudo python -mpip install numpy pyopengl Pillow pybind11 557 | 558 | git submodule init && git submodule update 559 | ``` 560 | 561 | #### 安装OpenCV 562 | ORB_SLAM2算法测试过OpenCV 3.2。之前安装了OpenCV 3.4，一般问题也不大，如果有问题，可以切回去试试。 563 | 564 | #### 安装Eigen3 565 | 至少需要3.1.0版本 566 | ``` 567 | sudo apt-get install libeigen3-dev 568 | ``` 569 | #### 安装ORB_SLAM2 570 | ``` 571 | mkdir -p ~/src 572 | 573 | cd ~/src 574 | 575 | git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2 576 | 577 | cd ORB_SLAM2 578 | 579 | chmod +x build.sh 580 | 581 | ./build.sh 582 | 583 | export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/src/ORB_SLAM2/Examples/ROS 584 | 585 | chmod +x build_ros.sh 586 | 587 | ./build_ros.sh 588 | ``` 589 | 至此编译完成。 590 | 591 | 注意: ORB_SLAM2默认ROS的topic为: 592 | ``` 593 | "/camera/left/image_raw" 594 | "/camera/right/image_raw" 595 | ``` 596 | 需更改为cooleye相机默认的: 597 | ``` 598 | "/cooleyed1/left/image_raw" 599 | "/cooleyed1/right/image_raw" 600 | ``` 601 | 602 | 需要更改的文件有: 603 | ``` 604 | Examples/ROS/ORB_SLAM2/src/ros_mono.cc 605 | Examples/ROS/ORB_SLAM2/src/ros_stereo.cc 606 | Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc 607 | Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc: 608 | ``` 609 | 610 | 611 | ### 3.2 okvis算法的安装和使用 612 | OKVIS: Open Keyframe-based Visual-Inertial SLAM. 也是常用的开源项目之一。 613 | 首先安装依赖包。 614 | ``` 615 | sudo apt-get install cmake libgoogle-glog-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libboost-dev libboost-filesystem-dev libopencv-dev 616 | ``` 617 | 下载okvis算法包 618 | ``` 619 | mkdir -p ~/src 620 | 621 | cd ~/src 622 | 623 | git clone https://github.com/ethz-asl/okvis.git 624 | 625 | cd ~/src/okvis 626 | 627 | mkdir build && cd build 628 | 629 | cmake -DCMAKE_BUILD_TYPE=Release .. 630 | 631 | make -j8 632 | 633 | make install 634 | ``` 635 | 至此，编译安装完成。 636 | 637 | 如果，如果遇到以下ERROR，这是由于ceres-solver无法安装导致的。 638 | ``` 639 | Failed to clone repository: 640 | 'https://ceres-solver.googlesource.com/ceres-solver' 641 | 642 | 643 | CMakeFiles/ceres_external.dir/build.make:89: recipe for target 'ceres/src/ceres_external-stamp/ceres_external-download' failed 644 | make[2]: *** [ceres/src/ceres_external-stamp/ceres_external-download] Error 1 645 | CMakeFiles/Makefile2:153: recipe for target 'CMakeFiles/ceres_external.dir/all' failed 646 | make[1]: *** [CMakeFiles/ceres_external.dir/all] Error 2 647 | make[1]: *** Waiting for unfinished jobs.... 648 | ``` 649 | 650 | __解决方案如下：__ 651 | 将okvis目录下CMakeLists.txt 中，OFF修改为ON 652 | ``` 653 | option (USE_SYSTEM_CERES 654 | "Use ceres via find_package rather than downloading it as part of okvis" OFF) 655 | 656 | 改为 657 | option (USE_SYSTEM_CERES 658 | "Use ceres via find_package rather than downloading it as part of okvis" ON) 659 | ``` 660 | 根据以下文档独立安装ceres 661 | ``` 662 | https://blog.csdn.net/xiat5/article/details/79164059 663 | ``` 664 | 删除okvis目录下的build文件夹，重新编译，问题应该就可以解决了。 665 | 666 | 667 | 668 | ### 3.3 vins算法的安装和使用 669 | 670 | vins是香港科技大学开源的一个单目相机结合IMU的一个VIO，在github上可以下载源码，分为iOS系统下的和ros系统下的两种.本相机当然是适用ROS版本. 671 | ``` 672 | https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git 673 | ``` 674 | 1. 根据提示安装ROS必备得依赖包 675 | ``` 676 | sudo apt-get install ros-kinetic-cv-bridge ros-kinetic-tf ros-kinetic-message-filters ros-kinetic-image-transport 677 | ``` 678 | 2. Ceres Solver是必备得组件,请参照下方教程安装最新版. 679 | ``` 680 | http://ceres-solver.org/installation.html#linux 681 | ``` 682 | 3. 按照官方指导编译VINS-Mono的开发环境 683 | ``` 684 | cd ~/catkin_ws/src 685 | git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git 686 | cd ../ 687 | catkin_make 688 | source ~/catkin_ws/devel/setup.bash 689 | ``` 690 | 4. 根据相机内参编写config和launch文件.即可运行VINS算法. 691 | 692 | ------------------------------------------------------------------------------------------------- 693 | 694 | ## 四. 相机固件升级方法 695 | 696 | ### 4.1 IMU操作固件升级 697 | 目前版本硬件，IMU的控制依赖于一个小型的单片机，如果需要升级，sdk/update会直接发布程序的HEX文件。windows下可使用FlyMcu进行升级，操作步骤见下图。另，熟悉STM32的用户，可根据自己习惯选择自己喜欢的升级方式。 698 | 699 | ![p1](https://ws4.sinaimg.cn/large/006tNc79gy1fs45hbvz2uj30lo0et410.jpg) 700 | 701 | ### 4.2 图像处理固件升级 702 | 目前版本硬件，图像Sensor的控制使用了Cypress的方案，sdk/update提供了windows下的驱动文件和升级工具，同时也会直接发布用于升级的IIC文件。升级步骤见下图。 703 | 如果设备在电脑端识别异常，请在设备管理器中卸载设备驱动，然后更新驱动指向driver文件夹，比如： 704 | D:\Cypress\USB\CY3684_EZ-USB_FX2LP_DVK\1.1\Drivers\vista 705 | 需要注意的是，这里只需指向vista即可，请勿继续选择其子目录x86或x64，可能会造成识别异常。 706 | 707 | ![p2](https://ws4.sinaimg.cn/large/006tNc79gy1fs45hhn7e2j30is0f3gow.jpg) 708 | 709 | 710 | ------------------------------------------------------------------------------------------------- 711 | 712 | ### 五. 常见问题汇总 713 | #### 5.1 串口无法打开 714 | - celog: uart open error !: Permission denied 715 | 716 | 新环境下串口需要root权限。请执行以下命令，其中yourusername为你的当前用户名。 717 | 718 | ``` 719 | sudo gpasswd -a yourusername dialout 720 | ``` 721 | 注销并且重新登录， 即可解决此问题。 722 | 723 | 724 | #### 5.2 ORB_SLAM2的ROS环境编译不通过 725 | 726 | 如果遇到以下问题编译不过 727 | ``` 728 | /usr/bin/ld: CMakeFiles/RGBD.dir/src/ros_rgbd.cc.o: undefined reference to symbol '_ZN5boost6system15system_categoryEv' 729 | /usr/lib/x86_64-linux-gnu/libboost_system.so: error adding symbols: DSO missing from command line 730 | collect2: error: ld returned 1 exit status 731 | CMakeFiles/RGBD.dir/build.make:220: recipe for target '../RGBD' failed 732 | make[2]: *** [../RGBD] Error 1 733 | CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/RGBD.dir/all' failed 734 | make[1]: *** [CMakeFiles/RGBD.dir/all] Error 2 735 | make[1]: *** Waiting for unfinished jobs.... 736 | /usr/bin/ld: CMakeFiles/Stereo.dir/src/ros_stereo.cc.o: undefined reference to symbol '_ZN5boost6system15system_categoryEv' 737 | /usr/lib/x86_64-linux-gnu/libboost_system.so: error adding symbols: DSO missing from command line 738 | collect2: error: ld returned 1 exit status 739 | CMakeFiles/Stereo.dir/build.make:220: recipe for target '../Stereo' failed 740 | make[2]: *** [../Stereo] Error 1 741 | CMakeFiles/Makefile2:104: recipe for target 'CMakeFiles/Stereo.dir/all' failed 742 | make[1]: *** [CMakeFiles/Stereo.dir/all] Error 2 743 | Makefile:127: recipe for target 'all' failed 744 | make: *** [all] Error 2 745 | ``` 746 | 解决方案为 747 | - __将libboost_system.so与libboost_filesystem.so复制到~/src/ORB_SLAM2/lib下__ 748 | ``` 749 | cp /usr/lib/x86_64-linux-gnu/libboost_system.a ~/src/ORB_SLAM2/lib/ 750 | cp /usr/lib/x86_64-linux-gnu/libboost_system.so ~/src/ORB_SLAM2/lib/ 751 | cp /usr/lib/x86_64-linux-gnu/libboost_system.so.1.58.0 ~/src/ORB_SLAM2/lib/ 752 | cp /usr/lib/x86_64-linux-gnu/libboost_filesystem.a ~/src/ORB_SLAM2/lib/ 753 | cp /usr/lib/x86_64-linux-gnu/libboost_filesystem.so ~/src/ORB_SLAM2/lib/ 754 | cp /usr/lib/x86_64-linux-gnu/libboost_filesystem.so.1.58.0 ~/src/ORB_SLAM2/lib/ 755 | ``` 756 | 如果没有libboost_system文件，可以运行以下命令查找 757 | ``` 758 | locate boost_system 759 | locate boost_filesystem 760 | ``` 761 | 762 | - __将~/src/ORBSLAM2/Examples/ROS/ORBSLAM2下的Cmakelists.txt中加入库目录__ 763 | ``` 764 | set(LIBS 765 | ${OpenCV_LIBS} 766 | ${EIGEN3_LIBS} 767 | ${Pangolin_LIBRARIES} 768 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so 769 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so 770 | ${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so 771 | #加入以下内容 772 | ${PROJECT_SOURCE_DIR}/../../../lib/libboost_filesystem.so 773 | ${PROJECT_SOURCE_DIR}/../../../lib/libboost_system.so 774 | ) 775 | ``` 776 | 777 | 778 | ### 六. 硬件尺寸图 779 | 下图说明了相机的IMU和双目之间的结构图关系，由于是正视图，因此左右相机正好相反，使用时注意： 780 | 781 | ![enter image description here](https://ws1.sinaimg.cn/large/006tKfTcgy1fs8og6na6pj318g0eu0tl.jpg) 782 | 783 | ------------------------------------------------------------------------------------------------- 784 | 785 | ### 修订历史 786 | - 2018-06-03手册首次发布 787 | - 2018-06-08增加相机升级教程，kalibr的安装教程，常见问题汇总 788 | - 2018-06-12增加okvis算法的安装 789 | - 2018-07-03增加视差输出,畸变矫正输出,增加opencv鱼眼标定程序 790 | - 2018-07-26增加VINS算法 791 | 792 | 793 | -------------------------------------------------------------------------------- /cooleye_d1.code-workspace: -------------------------------------------------------------------------------- 1 | { 2 | "folders": [ 3 | { 4 | "path": "sdk" 5 | } 6 | ] 7 | } -------------------------------------------------------------------------------- /sdk/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(camtest) 4 | 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -g") 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -g") 7 | 8 | 9 | find_package(OpenCV REQUIRED) 10 | # find opencv3 OpenCVConfig.cmake 11 | # set(OpenCV_DIR "/usr/local//share/OpenCV") 12 | # find_package(OpenCV 3.4.0 REQUIRED) 13 | #include_directories(${OpenCV_INCLUDE_DIRS}) 14 | 15 | 16 | include_directories("${PROJECT_SOURCE_DIR}/include") 17 | 18 | add_library(CELIB_BASE src/logmsg.cpp src/cpu_set.cpp) 19 | 20 | add_library(CEDRIVER_CONFIG src/cedriver_config.cpp) 21 | 22 | add_library(CEDRIVER_CAM src/cedriver_usb.cpp src/cedriver_cam.cpp) 23 | 24 | add_library(CELIB_MadgwickAHRS src/celib_MadgwickAHRS.cpp) 25 | 26 | add_library(CELIB_IMG_PROCESS src/celib_img_process.cpp) 27 | 28 | add_library(CEDRIVER_IMU src/cedriver_imu.cpp) 29 | target_link_libraries(CEDRIVER_IMU CELIB_MadgwickAHRS ) 30 | 31 | 32 | 33 | 34 | ## APP 35 | add_executable(CEAPP_RAW_DATA src/ceapp_raw_data.cpp) 36 | target_link_libraries( CEAPP_RAW_DATA 37 | CEDRIVER_CAM 38 | CEDRIVER_CONFIG 39 | CEDRIVER_IMU 40 | CELIB_BASE 41 | usb-1.0 42 | ${OpenCV_LIBS}) 43 | 44 | 45 | add_executable(CEAPP_WRITE_DEVICEID src/ceapp_write_deviceid.cpp) 46 | target_link_libraries( CEAPP_WRITE_DEVICEID 47 | CEDRIVER_CAM 48 | CEDRIVER_CONFIG 49 | CEDRIVER_IMU 50 | CELIB_BASE 51 | usb-1.0 52 | ${OpenCV_LIBS}) 53 | 54 | add_executable(CEAPP_STEREO_MATCH src/ceapp_stereo_match.cpp) 55 | target_link_libraries( CEAPP_STEREO_MATCH 56 | CEDRIVER_CAM 57 | CEDRIVER_CONFIG 58 | CEDRIVER_IMU 59 | CELIB_IMG_PROCESS 60 | CELIB_BASE 61 | usb-1.0 62 | ${OpenCV_LIBS}) 63 | 64 | add_executable(CEAPP_DISP_FILTER src/ceapp_disp_filter.cpp) 65 | target_link_libraries( CEAPP_DISP_FILTER 66 | CEDRIVER_CAM 67 | CEDRIVER_CONFIG 68 | CEDRIVER_IMU 69 | CELIB_MadgwickAHRS 70 | CELIB_IMG_PROCESS 71 | CELIB_BASE 72 | usb-1.0 73 | ${OpenCV_LIBS}) 74 | 75 | ## TOOLS 76 | add_executable(CETOOL_CALI_IMU src/cetool_cali_imu.cpp) 77 | target_link_libraries( CETOOL_CALI_IMU 78 | CEDRIVER_IMU 79 | CEDRIVER_CONFIG 80 | CELIB_BASE 81 | ${OpenCV_LIBS}) 82 | 83 | add_executable(CETOOL_STEREO_CAPTURE_IMG src/cetool_cali_stereo_capture_img.cpp) 84 | target_link_libraries( CETOOL_STEREO_CAPTURE_IMG 85 | CEDRIVER_CAM 86 | CEDRIVER_CONFIG 87 | CEDRIVER_IMU 88 | CELIB_IMG_PROCESS 89 | CELIB_BASE 90 | usb-1.0 91 | ${OpenCV_LIBS}) 92 | 93 | add_executable(CETOOL_CALI_STEREO_CAL src/cetool_cali_stereo_cal.cpp) 94 | target_link_libraries( CETOOL_CALI_STEREO_CAL 95 | CEDRIVER_CAM 96 | CEDRIVER_CONFIG 97 | CEDRIVER_IMU 98 | CELIB_BASE 99 | usb-1.0 100 | ${OpenCV_LIBS}) 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /sdk/config/cecfg_std.txt: -------------------------------------------------------------------------------- 1 | ################################### 2 | ################################### 3 | #### global_dev_config 4 | #### 5 | cf_dev_name=CoolEyeD1; 6 | cf_dev_version=V1.0; 7 | # 8 | ################################### 9 | ################################### 10 | # 11 | ##################################### 12 | ##################################### 13 | #### global_imu_config 14 | #### 15 | # calibration 1=Enable 0 = disable; 16 | cf_imu_uart=/dev/ttyUSB0; 17 | cf_imu_icm20689_acc_bias_X=328.476562; 18 | cf_imu_icm20689_acc_bias_Y=-40.180664; 19 | cf_imu_icm20689_acc_bias_Z=2.004883; 20 | cf_imu_icm20689_gyro_bias_X=69.083008; 21 | cf_imu_icm20689_gyro_bias_Y=318.983582; 22 | cf_imu_icm20689_gyro_bias_Z=7.027148; 23 | cf_imu_icm20689_acc_T[0][0]=1.001493; 24 | cf_imu_icm20689_acc_T[0][1]=0.001994; 25 | cf_imu_icm20689_acc_T[0][2]=0.030158; 26 | cf_imu_icm20689_acc_T[1][0]=0.039036; 27 | cf_imu_icm20689_acc_T[1][1]=1.001209; 28 | cf_imu_icm20689_acc_T[1][2]=0.011952; 29 | cf_imu_icm20689_acc_T[2][0]=-0.019433; 30 | cf_imu_icm20689_acc_T[2][1]=-0.000653; 31 | cf_imu_icm20689_acc_T[2][2]=0.981460; 32 | cf_imu_icm20689_sample_rate=1000; 33 | ###################################### 34 | ###################################### 35 | # 36 | ################################### 37 | ################################### 38 | #### global_cam_config 39 | #### 40 | #### 41 | #mode : left=1;right =2;streoe =3; 42 | cf_cam_mode=3; 43 | #VGA = 1 WVGA = 2 44 | cf_cam_resolution=2; 45 | # FPS: 1~45 is option. 46 | cf_cam_FPS=20; 47 | ##EG_MODE 48 | # 0 : Manual mode 49 | # 1 : AEC/AGC mode 50 | # 2 : AEC / Manual Gain 51 | # 3 : default and do nothing 52 | # 4 : just set skip frame 53 | cf_cam_EG_mode=1; 54 | # 0 : Manual mode ####### 55 | cf_cam_man_exp=50; 56 | cf_cam_man_gain=25; 57 | # 1 : AEC/AGC mode######### 58 | cf_cam_auto_EG_top=200; 59 | cf_cam_auto_EG_bottom=5; 60 | cf_cam_auto_EG_des=158; 61 | # 2 : AEC / Manual Gain#### 62 | cf_cam_auto_E_man_G_Etop=300; 63 | cf_cam_auto_E_man_G_Ebottom=4; 64 | cf_cam_auto_E_man_G=500; 65 | # 3 : default and do nothing##### 66 | # 4 : just set skip frame ####### 67 | cf_cam_agc_aec_skip_frame=0; 68 | ##################################### 69 | ## 0 = raw 1= rectify 70 | cf_cam_rectify=0; 71 | ##################################### 72 | ##################################### 73 | cf_ros_showimage=0; 74 | ##################################### 75 | #stereo_match_algorithm 76 | #STEREO_BM=0 77 | #STEREO_SGBM=1 78 | #STEREO_HH=2 79 | #STEREO_3WAY=4 80 | ################################### 81 | cf_ste_algorithm=2; 82 | -------------------------------------------------------------------------------- /sdk/config/cooleye_cam_set.sh: -------------------------------------------------------------------------------- 1 | create_udev_rules() { 2 | echo "" > cooleye_d1_cam.rules 3 | echo 'KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="add", ATTR{idVendor}=="04b4",ATTR{idProduct}="1003", MODE="666" ' >> cooleye_d1_cam.rules 4 | echo 'KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="remove" ' >> cooleye_d1_cam.rules 5 | } 6 | 7 | if [ `whoami` != 'root' ]; then 8 | echo "You have to be root to run this script" 9 | echo "请使用root权限运行此脚本" 10 | exit 1; 11 | fi 12 | 13 | create_udev_rules 14 | cp cooleye_d1_cam.rules /etc/udev/rules.d/ 15 | rm cooleye_d1_cam.rules 16 | -------------------------------------------------------------------------------- /sdk/config/extrinsics.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | R: !!opencv-matrix 4 | rows: 3 5 | cols: 3 6 | dt: d 7 | data: [ 9.9998308755043197e-01, -5.4433451098753098e-03, 8 | -2.0480740025299662e-03, 5.4314300644049711e-03, 9 | 9.9996855115738847e-01, -5.7789500472666859e-03, 10 | 2.0794664124530538e-03, 5.7677283403539713e-03, 11 | 9.9998120438797811e-01 ] 12 | T: !!opencv-matrix 13 | rows: 3 14 | cols: 1 15 | dt: d 16 | data: [ -1.2006399127573071e+02, -5.9553946397632118e-02, 17 | 1.5378350158021134e-01 ] 18 | R1: !!opencv-matrix 19 | rows: 3 20 | cols: 3 21 | dt: d 22 | data: [ 9.9998217487971908e-01, -4.9547252283659981e-03, 23 | -3.3317594058251340e-03, 4.9450934362659273e-03, 24 | 9.9998358830210932e-01, -2.8929530490558928e-03, 25 | 3.3460385134529355e-03, 2.8764256202508708e-03, 26 | 9.9999026505357425e-01 ] 27 | R2: !!opencv-matrix 28 | rows: 3 29 | cols: 3 30 | dt: d 31 | data: [ 9.9999905670078193e-01, 4.9601791168955953e-04, 32 | -1.2808449467740534e-03, -4.9232099078206455e-04, 33 | 9.9999571713594737e-01, 2.8850181636081293e-03, 34 | 1.2822704817739517e-03, -2.8843848553195095e-03, 35 | 9.9999501804079904e-01 ] 36 | P1: !!opencv-matrix 37 | rows: 3 38 | cols: 4 39 | dt: d 40 | data: [ 3.0538266499637734e+02, 0., 3.8508162553219563e+02, 0., 0., 41 | 3.0538266499637734e+02, 2.3513107581500662e+02, 0., 0., 0., 1., 42 | 0. ] 43 | P2: !!opencv-matrix 44 | rows: 3 45 | cols: 4 46 | dt: d 47 | data: [ 3.0538266499637734e+02, 0., 3.8508162553219563e+02, 48 | -3.6665496212418358e+04, 0., 3.0538266499637734e+02, 49 | 2.3513107581500662e+02, 0., 0., 0., 1., 0. ] 50 | Q: !!opencv-matrix 51 | rows: 4 52 | cols: 4 53 | dt: d 54 | data: [ 1., 0., 0., -3.8508162553219563e+02, 0., 1., 0., 55 | -2.3513107581500662e+02, 0., 0., 0., 3.0538266499637734e+02, 0., 56 | 0., 8.3288840065649048e-03, 0. ] 57 | -------------------------------------------------------------------------------- /sdk/config/intrinsics.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | M1: !!opencv-matrix 4 | rows: 3 5 | cols: 3 6 | dt: d 7 | data: [ 3.6389055885584037e+02, 0., 3.8575665548117576e+02, 0., 8 | 3.6389106568198576e+02, 2.3928264773121046e+02, 0., 0., 1. ] 9 | D1: !!opencv-matrix 10 | rows: 4 11 | cols: 1 12 | dt: d 13 | data: [ -1.9999731736073709e-02, -1.2218853033449823e-02, 14 | 1.8398645637088450e-02, -1.2312755172117123e-02 ] 15 | M2: !!opencv-matrix 16 | rows: 3 17 | cols: 3 18 | dt: d 19 | data: [ 3.6608707088309507e+02, 0., 3.7136525474139387e+02, 0., 20 | 3.6604112958185721e+02, 2.3378627901248655e+02, 0., 0., 1. ] 21 | D2: !!opencv-matrix 22 | rows: 4 23 | cols: 1 24 | dt: d 25 | data: [ -1.9391146531354306e-02, -5.8908201928704919e-03, 26 | -1.9389390313359649e-04, 9.6099298343147463e-04 ] 27 | -------------------------------------------------------------------------------- /sdk/config/serial.txt: -------------------------------------------------------------------------------- 1 | serial_D=1 2 | serial_S=1 3 | -------------------------------------------------------------------------------- /sdk/config/stereo_calib.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | "../build/left1.jpg" 5 | "../build/right1.jpg" 6 | "../build/left2.jpg" 7 | "../build/right2.jpg" 8 | "../build/left3.jpg" 9 | "../build/right3.jpg" 10 | "../build/left4.jpg" 11 | "../build/right4.jpg" 12 | "../build/left5.jpg" 13 | "../build/right5.jpg" 14 | "../build/left6.jpg" 15 | "../build/right6.jpg" 16 | "../build/left7.jpg" 17 | "../build/right7.jpg" 18 | "../build/left8.jpg" 19 | "../build/right8.jpg" 20 | "../build/left9.jpg" 21 | "../build/right9.jpg" 22 | "../build/left10.jpg" 23 | "../build/right10.jpg" 24 | "../build/left11.jpg" 25 | "../build/right11.jpg" 26 | "../build/left12.jpg" 27 | "../build/right12.jpg" 28 | "../build/left13.jpg" 29 | "../build/right13.jpg" 30 | "../build/left14.jpg" 31 | "../build/right14.jpg" 32 | "../build/left15.jpg" 33 | "../build/right15.jpg" 34 | "../build/left16.jpg" 35 | "../build/right16.jpg" 36 | "../build/left17.jpg" 37 | "../build/right17.jpg" 38 | "../build/left18.jpg" 39 | "../build/right18.jpg" 40 | "../build/left19.jpg" 41 | "../build/right19.jpg" 42 | "../build/left20.jpg" 43 | "../build/right20.jpg" 44 | "../build/left21.jpg" 45 | "../build/right21.jpg" 46 | "../build/left22.jpg" 47 | "../build/right22.jpg" 48 | "../build/left23.jpg" 49 | "../build/right23.jpg" 50 | "../build/left24.jpg" 51 | "../build/right24.jpg" 52 | "../build/left25.jpg" 53 | "../build/right25.jpg" 54 | "../build/left26.jpg" 55 | "../build/right26.jpg" 56 | "../build/left27.jpg" 57 | "../build/right27.jpg" 58 | "../build/left28.jpg" 59 | "../build/right28.jpg" 60 | "../build/left29.jpg" 61 | "../build/right29.jpg" 62 | "../build/left30.jpg" 63 | "../build/right30.jpg" 64 | "../build/left31.jpg" 65 | "../build/right31.jpg" 66 | "../build/left32.jpg" 67 | "../build/right32.jpg" 68 | "../build/left33.jpg" 69 | "../build/right33.jpg" 70 | "../build/left34.jpg" 71 | "../build/right34.jpg" 72 | "../build/left35.jpg" 73 | "../build/right35.jpg" 74 | "../build/left36.jpg" 75 | "../build/right36.jpg" 76 | "../build/left37.jpg" 77 | "../build/right37.jpg" 78 | "../build/left38.jpg" 79 | "../build/right38.jpg" 80 | "../build/left39.jpg" 81 | "../build/right39.jpg" 82 | "../build/left40.jpg" 83 | "../build/right40.jpg" 84 | "../build/left41.jpg" 85 | "../build/right41.jpg" 86 | "../build/left42.jpg" 87 | "../build/right42.jpg" 88 | "../build/left43.jpg" 89 | "../build/right43.jpg" 90 | "../build/left44.jpg" 91 | "../build/right44.jpg" 92 | "../build/left45.jpg" 93 | "../build/right45.jpg" 94 | "../build/left46.jpg" 95 | "../build/right46.jpg" 96 | "../build/left47.jpg" 97 | "../build/right47.jpg" 98 | "../build/left48.jpg" 99 | "../build/right48.jpg" 100 | "../build/left49.jpg" 101 | "../build/right49.jpg" 102 | "../build/left50.jpg" 103 | "../build/right50.jpg" 104 | "../build/left45.jpg" 105 | "../build/right45.jpg" 106 | "../build/left46.jpg" 107 | "../build/right46.jpg" 108 | "../build/left47.jpg" 109 | "../build/right47.jpg" 110 | "../build/left48.jpg" 111 | "../build/right48.jpg" 112 | "../build/left49.jpg" 113 | "../build/right49.jpg" 114 | "../build/left50.jpg" 115 | "../build/right50.jpg" 116 | "../build/left51.jpg" 117 | "../build/right51.jpg" 118 | "../build/left52.jpg" 119 | "../build/right52.jpg" 120 | "../build/left53.jpg" 121 | "../build/right53.jpg" 122 | "../build/left54.jpg" 123 | "../build/right54.jpg" 124 | "../build/left55.jpg" 125 | "../build/right55.jpg" 126 | "../build/left56.jpg" 127 | "../build/right56.jpg" 128 | "../build/left57.jpg" 129 | "../build/right57.jpg" 130 | "../build/left58.jpg" 131 | "../build/right58.jpg" 132 | "../build/left59.jpg" 133 | "../build/right59.jpg" 134 | "../build/left60.jpg" 135 | "../build/right60.jpg" 136 | "../build/left61.jpg" 137 | "../build/right61.jpg" 138 | "../build/left62.jpg" 139 | "../build/right62.jpg" 140 | "../build/left63.jpg" 141 | "../build/right63.jpg" 142 | "../build/left64.jpg" 143 | "../build/right64.jpg" 144 | "../build/left65.jpg" 145 | "../build/right65.jpg" 146 | "../build/left66.jpg" 147 | "../build/right66.jpg" 148 | "../build/left67.jpg" 149 | "../build/right67.jpg" 150 | "../build/left68.jpg" 151 | "../build/right68.jpg" 152 | "../build/left69.jpg" 153 | "../build/right69.jpg" 154 | "../build/left70.jpg" 155 | "../build/right70.jpg" 156 | 157 | 158 | -------------------------------------------------------------------------------- /sdk/config/updata_rosfile.sh: -------------------------------------------------------------------------------- 1 | mkdir -p ../../ros/src/cooleye_d1 2 | cp -rf ../include ../../ros/src/cooleye_d1/ 3 | cp -rf ../src ../../ros/src/cooleye_d1/ 4 | cp -rf ../config ../../ros/src/cooleye_d1/ 5 | cp -rf ../rosfile/CMakeLists.txt ../../ros/src/cooleye_d1/ 6 | cp -rf ../rosfile/package.xml ../../ros/src/cooleye_d1/ 7 | -------------------------------------------------------------------------------- /sdk/dso_config/camera.txt: -------------------------------------------------------------------------------- 1 | RadTan 363.89 363.89 385.75 239.28 -0.019999 -0.012218 0.018398 -0.0123127 2 | 752 480 3 | none 4 | 752 480 5 | 6 | -------------------------------------------------------------------------------- /sdk/dso_config/pcalib.txt: -------------------------------------------------------------------------------- 1 | 0 0.361816590639235 0.422955615274744 0.498937941433628 0.587103908694196 0.686564311238687 0.796271612228055 0.914210347687387 1.03661440136014 1.16110234527017 1.28716437066554 1.41311583435901 1.53907585585680 1.66890859220358 1.80476116614550 1.95090298478911 2.10758432417637 2.27076571579657 2.43840681238287 2.60935843212962 2.78393470905777 2.96163021154753 3.14458607228658 3.33255572429063 3.52693618789227 3.72755964533922 3.93368048580260 4.14652695310964 4.36631933248305 4.59341978651250 4.82603217286584 5.06462022055344 5.30736350968266 5.55365256175861 5.79981334918349 6.04554959584991 6.28922282617138 6.53257690631450 6.77310648909376 7.01737179843069 7.25880100012592 7.50210693322409 7.74462956915910 7.99107902555889 8.23658993839751 8.48605093440551 8.73437708067164 8.98502272720992 9.23477914405921 9.48499696460253 9.73185804334373 9.97889053201734 10.2213314823002 10.4649473008804 10.7068836196297 10.9479052156666 11.1907218687848 11.4326357472046 11.6760376796436 11.9190142485837 12.1631417688062 12.4059916554862 12.6474783172904 12.8911163495743 13.1301493004318 13.3680169523340 13.6054693518292 13.8407855627645 14.0771704100060 14.3116065823854 14.5450969630336 14.7812597117143 15.0150407356343 15.2526984055773 15.4859887834262 15.7191843287214 15.9527987199729 16.1852120643182 16.4178062060816 16.6507643334959 16.8826351395204 17.1142935621074 17.3477457916097 17.5789073309354 17.8121463003004 18.0434742192390 18.2751870875228 18.5086154442201 18.7402995404677 19.2023530958312 20.0169068517838 20.9574035495430 21.9043977159540 22.8585623383324 23.8173700045193 24.7783559347418 25.7359002355130 26.6888410873342 27.6512062745192 28.6338481635429 29.6331838665039 30.6376361510687 31.6401630166108 32.6450661565620 33.6466186000048 34.6421424575353 35.6323940218821 36.6284429206016 37.6324463505231 38.6398585108271 39.6538524532584 40.6637692149940 41.6655761095527 42.6593953235910 43.6500387790159 44.6417921210861 45.6358887379688 46.6333953327726 47.6287990148780 48.6242163937663 49.6210990698482 50.6209636033713 51.6194697948538 52.6151045036130 53.6094168788667 54.6061047930164 55.6065672492900 56.6056207103128 57.6086811831108 58.6106584965392 59.6096150987057 60.6130145952724 61.6151099687242 62.6195358277765 63.6272940491089 64.6356039250690 65.6410806406787 66.6503375471232 67.6600973217814 68.6716176542411 69.6833736123801 70.6953709234252 71.7044964824533 72.7143482433981 73.7282069913642 74.7509733950528 75.7642704553978 76.7786444535425 77.7956281651977 78.8131395333657 79.8316404431632 80.8499266307603 81.8717042929930 82.8927512089468 83.9162258027827 84.9382002198668 85.9626917059902 86.9833460585508 88.0030840354758 89.0207518775425 90.0404106820213 91.0681528318190 92.0953779773317 93.1188356560578 94.1398877462103 95.1655513944157 96.1933125812296 97.2220315195179 98.2466868408547 99.2685627436670 100.293820364142 101.326463518034 102.354335098173 103.382812307200 104.413294072945 105.449882601198 106.481926740673 107.509522379255 108.531076231016 109.555674550863 110.591491179774 111.635027360510 112.664643503516 113.691600439063 114.716388289893 115.750192107938 116.782772550077 117.821051874147 118.856984425433 120.411685563105 122.483987222510 124.561436720237 126.623771548890 128.699180273485 130.797169679232 132.879690892346 134.958333467952 137.043622036889 139.110848879217 141.158591483001 143.223022605973 145.290785374665 147.341417544414 149.417785623493 151.517050243467 153.606604230907 155.655407412890 157.699166123050 159.741295837699 161.791357881778 163.859257478491 165.951709312656 168.032705829540 170.107389489806 172.195748155303 174.306873767736 176.409995659962 178.500464079479 180.593721242261 182.707780901629 184.815548608478 186.912091284153 188.998253223930 191.091869132510 193.200810209726 195.303286111641 197.396870833291 199.483131531296 201.591119387463 203.704666751383 205.808282811758 207.902873329693 209.989793096523 212.089126960371 214.193970417273 216.306138776385 218.417219345709 220.523659790494 222.628354475153 224.727157756727 226.819507320919 228.910790743184 231.008657322406 233.117844915557 235.241642445630 237.356401622363 239.462934002225 241.566683757002 243.654765297381 245.722535910752 247.697883488748 249.457339786241 250.977114290911 252.350902581155 253.675451290577 255 2 | -------------------------------------------------------------------------------- /sdk/include/cedriver_cam.h: -------------------------------------------------------------------------------- 1 | #ifndef CEDRIVER_CAM_H 2 | #define CEDRIVER_CAM_H 3 | 4 | #include "threadsafe_queue.h" 5 | 6 | struct img_pkg 7 | { 8 | unsigned char data[IMG_BUF_SIZE_WVGA]; 9 | double timestamp; 10 | }; 11 | 12 | struct d1_img_output_pkg 13 | { 14 | img_pkg *left_img; 15 | img_pkg *right_img; 16 | }; 17 | 18 | struct s1_img_output_pkg 19 | { 20 | img_pkg *s1_img; 21 | }; 22 | 23 | typedef struct ce_cams1_dev_s 24 | { 25 | libusb_device_handle *handle; 26 | pthread_t thread; 27 | threadsafe_queue list; 28 | bool read_error_flag; 29 | int cam; 30 | 31 | ce_cams1_dev_s() 32 | { 33 | handle = NULL; 34 | thread = 0; 35 | read_error_flag = false; 36 | cam = 0; 37 | } 38 | }ce_cams1_dev_t; 39 | 40 | typedef struct ce_camd1_dev_s 41 | { 42 | libusb_device_handle *handle; 43 | pthread_t thread; 44 | threadsafe_queue list; 45 | bool read_error_flag; 46 | int cam; 47 | 48 | ce_camd1_dev_s() 49 | { 50 | handle = NULL; 51 | thread = 0; 52 | read_error_flag = false; 53 | cam = 0; 54 | } 55 | }ce_camd1_dev_t; 56 | 57 | /* D1 */ 58 | int ce_cam_capture_init(); 59 | void ce_cam_capture_close(); 60 | int ce_cam_showimg_init(); 61 | void ce_cam_showimg_close(); 62 | int ce_cam_preprocess_init(); 63 | void ce_cam_preprocess_close(); 64 | 65 | /* S1 */ 66 | int ce_cams1_capture_init(); 67 | int ce_cams1_preprocess_init(); 68 | void ce_cams1_preprocess_close(); 69 | 70 | int ce_cam_write_deviceid_D1(); 71 | int ce_cam_write_deviceid_S1(); 72 | 73 | #if 1 74 | #define CE_COUT std::cout 75 | #else 76 | #define CE_COUT g_PrintfFile 77 | #endif 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /sdk/include/cedriver_config.h: -------------------------------------------------------------------------------- 1 | #ifndef CEDRIVER_CONFIG_H 2 | #define CEDRIVER_CONFIG_H 3 | 4 | void ce_config_load_settings(const char* settings_file); 5 | 6 | std::string ce_config_get_cf_dev_name(); 7 | std::string ce_config_get_cf_dev_version(); 8 | std::string ce_config_get_cf_imu_uart(); 9 | 10 | int ce_config_get_cf_imu_icm20689_acc_bias_X(); 11 | int ce_config_get_cf_imu_icm20689_acc_bias_Y(); 12 | int ce_config_get_cf_imu_icm20689_acc_bias_Z(); 13 | int ce_config_get_cf_imu_icm20689_gyro_bias_X(); 14 | int ce_config_get_cf_imu_icm20689_gyro_bias_Y(); 15 | int ce_config_get_cf_imu_icm20689_gyro_bias_Z(); 16 | void ce_config_get_cf_imu_icm20689_acc_T(float **mat); 17 | int ce_config_get_cf_imu_icm20689_sample_rate(); 18 | 19 | int ce_config_get_cf_cam_mode(); 20 | int ce_config_get_cf_cam_resolution(); 21 | int ce_config_get_cf_cam_FPS(); 22 | int ce_config_get_cf_cam_EG_mode(); 23 | int ce_config_get_cf_cam_man_exp(); 24 | int ce_config_get_cf_cam_man_gain(); 25 | int ce_config_get_cf_cam_auto_EG_top(); 26 | int ce_config_get_cf_cam_auto_EG_bottom(); 27 | int ce_config_get_cf_cam_auto_EG_des(); 28 | int ce_config_get_cf_cam_auto_E_man_G_Etop(); 29 | int ce_config_get_cf_cam_auto_E_man_G_Ebottom(); 30 | int ce_config_get_cf_cam_auto_E_man_G(); 31 | int ce_config_get_cf_cam_agc_aec_skip_frame(); 32 | int ce_config_get_cf_cam_rectify(); 33 | int ce_config_get_cf_cam_rectify_force_on(); 34 | int ce_config_get_cf_cam_rectify_force_off(); 35 | 36 | std::string ce_config_get_cf_cam_intrinsics(); 37 | std::string ce_config_get_cf_cam_extrinsics(); 38 | 39 | 40 | int ce_config_get_cf_img_width(); 41 | int ce_config_get_cf_img_height(); 42 | int ce_config_get_cf_img_size(); 43 | int ce_config_get_cf_img_buff_size(); 44 | int ce_config_get_cf_img_HB(); 45 | int ce_config_get_cf_img_VB(); 46 | double ce_config_get_cf_img_time_offset(); 47 | 48 | int ce_config_get_cf_ros_showimage(); 49 | int ce_config_get_cf_ste_algorithm(); 50 | 51 | void ce_config_rst_imu_offset(void); 52 | void ce_config_rewrite_imu_offset(std::string filepath, float *gyro_offs, float *accel_offs, float **accel_T); 53 | 54 | 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /sdk/include/cedriver_global_config.h: -------------------------------------------------------------------------------- 1 | #ifndef CEDRIVER_GLOBAL_CONFIG_H 2 | #define CEDRIVER_GLOBAL_CONFIG_H 3 | 4 | #include 5 | 6 | //#define USING_RING_QUEUE 0 7 | ////////////////////////////////////////////////////////////////////////// 8 | // system define 9 | ////////////////////////////////////////////////////////////////////////// 10 | 11 | #define SUCCESS 0 12 | #define ERROR -1 13 | #define ERROR_LOST_LEFT_CAM -2 14 | #define ERROR_LOST_RIGHT_CAM -3 15 | 16 | ////////////////////////////////////////////////////////////////////////// 17 | ////// CAM D1 USB setting 18 | ////////////////////////////////////////////////////////////////////////// 19 | 20 | #define PID_CE_D1 0x1003 21 | #define VID_CE_D1 0x04B4 22 | 23 | #define MAXDEVICES 5 24 | 25 | #define RT_H2D 0x40 26 | #define RT_D2H 0xC0 27 | 28 | ////////////////////////////////////////////////////////////////////////// 29 | ////// CAM D1 IMU setting 30 | ////////////////////////////////////////////////////////////////////////// 31 | #define IMU_FRAME_LEN 16 32 | #define IMU_FIFO_SIZE 50 33 | #define IMU_DELAY_OFFSET (double)0.00065 34 | 35 | 36 | 37 | #define CMD_HEAD1 0x55 38 | #define CMD_HEAD2 0xAA 39 | #define CMD_IMC20689_START 0x02 40 | #define CMD_IMC20689_SAMPLE_RATE 0x04 41 | #define STD_g 9.80665 42 | ////////////////////////////////////////////////////////////////////////// 43 | ////// CAM D1 CAM setting 44 | ////////////////////////////////////////////////////////////////////////// 45 | #define CAMD1_SYS_CLKIN 27000000 46 | //#define CAMD1_SYS_CLKIN 26666666 47 | 48 | 49 | #define CAMD1_RIGHT 0xF0 50 | #define CAMD1_LEFT 0xF1 51 | #define CAMD1_FIRST CAMD1_RIGHT 52 | #define CAMD1_LAST CAMD1_LEFT 53 | #define CAMD1_CNT_MAX 2 54 | 55 | #define CAMD1_RIGHT_IDX (CAMD1_RIGHT-CAMD1_FIRST) 56 | #define CAMD1_LEFT_IDX (CAMD1_LEFT-CAMD1_FIRST) 57 | 58 | 59 | 60 | #define CAMS1_N0 0xB0 61 | #define CAMS1_N1 0xB1 62 | #define CAMS1_N2 0xB2 63 | #define CAMS1_N3 0xB3 64 | #define CAMS1_N4 0xB4 65 | #define CAMS1_N5 0xB5 66 | #define CAMS1_N6 0xB6 67 | #define CAMS1_N7 0xB7 68 | #define CAMS1_FIRST CAMS1_N0 69 | #define CAMS1_LAST CAMS1_N7 70 | #define CAMS1_CNT_MAX 8 71 | 72 | 73 | #define CE_GET_CAM_L_R_STRING(n) n==CAMD1_LEFT ? "left" : n==CAMD1_RIGHT ? "right" : "unknown" 74 | 75 | #define CAMD1_RESOLUTION_VGA 1 76 | #define CAMD1_RESOLUTION_WVGA 2 77 | 78 | #define CAMD1_LEFT_ENABLE 0x01 79 | #define CAMD1_RIGHT_ENABLE 0x02 80 | 81 | /*DEVICE ID MACRO*/ 82 | #define CE_DEVICEID_LEN 16 83 | #define CE_DEVICEID_MEMORY_ADDR 0xFF00 84 | 85 | #define CE_CAM_TYPE_D '1' 86 | #define CE_CAM_TYPE_S '3' 87 | 88 | #define CE_CAM_SUB_TYPE_D1_V1P6 "001" 89 | #define CE_CAM_SUB_TYPE_S1_V2P0 "001" 90 | 91 | #define CE_CAM_SERIAL_LEN 5 92 | 93 | /*PARAMETER MACRO*/ 94 | #define IMG_WIDTH_VGA 640 95 | #define IMG_HEIGHT_VGA 480 96 | #define IMG_SIZE_VGA (IMG_WIDTH_VGA*IMG_HEIGHT_VGA) 97 | #define IMG_BUF_SIZE_VGA (IMG_SIZE_VGA+0x200) 98 | #define IMG_WIDTH_WVGA 752 99 | #define IMG_HEIGHT_WVGA 480 100 | #define IMG_SIZE_WVGA (IMG_WIDTH_WVGA*IMG_HEIGHT_WVGA) 101 | #define IMG_BUF_SIZE_WVGA (IMG_SIZE_WVGA+0x200) 102 | #define FRAME_CLUST 54 103 | 104 | /*CAMERA CONTROL INSTRUCTION MACRO*/ 105 | #define SET_MCLK_48MHz 0xA1 106 | #define SET_MCLK_24MHz 0xA2 107 | #define STANDBY_SHORT 0xA3 108 | #define STANDBY_LONG 0xA4 109 | #define GET_CAM_LR 0xA5 110 | 111 | #define GET_THE_SOFT_VISION 0xA6 112 | 113 | #define CAM_I2C_R 0xA7 114 | #define CAM_I2C_W 0xA8 115 | #define CAM_DEVICEID 0xA9 116 | 117 | #define CAM_RESET 0xAC 118 | 119 | #define auto_expo 1 120 | #define EXP_VAL 50 121 | #define GAI_VAL 100 122 | 123 | #define CAM_I2C_ADDR 0x5C //0x5C 124 | 125 | #define VSCEL_I2C_ADDR 0X40 126 | 127 | 128 | ////////////////////////////////////////////////////////////////////////////////// 129 | // 130 | // config.txt struct 131 | ////////////////////////////////////////////////////////////////////////////////// 132 | 133 | typedef struct 134 | { 135 | std::string cf_dev_name; // D1 136 | std::string cf_dev_version; // V1.0 reserved 137 | }global_dev_config; 138 | 139 | typedef struct 140 | { 141 | int cf_cam_mode; // left - 2b'01 / right-2b'10 /sterero-2b'11 142 | int cf_cam_resolution; // 143 | int cf_cam_FPS; // FPS 144 | int cf_cam_EG_mode; 145 | int cf_cam_man_exp; 146 | int cf_cam_man_gain; 147 | int cf_cam_auto_EG_top; 148 | int cf_cam_auto_EG_bottom; 149 | int cf_cam_auto_EG_des; 150 | int cf_cam_auto_E_man_G_Etop; 151 | int cf_cam_auto_E_man_G_Ebottom; 152 | int cf_cam_auto_E_man_G; 153 | int cf_cam_agc_aec_skip_frame; 154 | int cf_cam_rectify; 155 | std::string cf_cam_intrinsics; 156 | std::string cf_cam_extrinsics; 157 | }global_cam_config; 158 | 159 | typedef struct 160 | { 161 | std::string cf_imu_uart; 162 | float cf_imu_icm20689_acc_bias_X; 163 | float cf_imu_icm20689_acc_bias_Y; 164 | float cf_imu_icm20689_acc_bias_Z; 165 | float cf_imu_icm20689_gyro_bias_X; 166 | float cf_imu_icm20689_gyro_bias_Y; 167 | float cf_imu_icm20689_gyro_bias_Z; 168 | float cf_imu_icm20689_acc_T[3][3]; 169 | int cf_imu_icm20689_sample_rate; 170 | }global_imu_config; 171 | 172 | typedef struct 173 | { 174 | int cf_img_width; 175 | int cf_img_height; 176 | int cf_img_size; 177 | int cf_img_buff_size; 178 | int cf_img_HB; 179 | int cf_img_VB; 180 | double cf_img_time_offset; 181 | }global_img_config; 182 | 183 | 184 | typedef struct 185 | { 186 | int cf_ros_showimage; 187 | }global_ros_config; 188 | 189 | 190 | typedef struct 191 | { 192 | int cf_ste_algorithm; 193 | }global_ste_config; 194 | 195 | 196 | typedef struct 197 | { 198 | global_dev_config gc_dev; 199 | global_cam_config gc_cam; 200 | global_imu_config gc_imu; 201 | global_img_config gc_img; 202 | global_ros_config gc_ros; 203 | global_ste_config gc_ste; 204 | 205 | }global_config_d1; 206 | 207 | 208 | 209 | 210 | #endif 211 | -------------------------------------------------------------------------------- /sdk/include/cedriver_imu.h: -------------------------------------------------------------------------------- 1 | #ifndef CEDRIVER_IMU_H 2 | #define CEDRIVER_IMU_H 3 | 4 | typedef struct 5 | { 6 | unsigned char num; 7 | float rx,ry,rz; 8 | float ax,ay,az; 9 | double timestamp; 10 | }icm20689_pkg; 11 | 12 | int ce_imu_capture_init(); 13 | void ce_imu_capture_close(); 14 | 15 | int ce_imu_showdata_init(); 16 | void ce_imu_showdata_close(); 17 | 18 | void ce_imu_get_acc_data(short int *ptr); 19 | void ce_imu_get_gyro_data(short int *ptr); 20 | #endif 21 | -------------------------------------------------------------------------------- /sdk/include/cedriver_usb.h: -------------------------------------------------------------------------------- 1 | #ifndef CEDRIVER_USB_H 2 | #define CEDRIVER_USB_H 3 | 4 | #include "cedriver_global_config.h" 5 | #include 6 | 7 | struct ce_usb_dev_stru 8 | { 9 | libusb_device *dev; 10 | libusb_device_handle *handle; 11 | unsigned short vid; 12 | unsigned short pid; /* Product ID */ 13 | unsigned char is_open; /* When device is opened, val = 1 */ 14 | unsigned char busnum; /* The bus number of this device */ 15 | unsigned char devaddr; /* The device address*/ 16 | unsigned char filler; /* Padding to make struct = 16 bytes */ 17 | }; 18 | 19 | int ce_usb_open(void); 20 | void ce_usb_close(void); 21 | void ce_usb_close(int i); 22 | 23 | libusb_device_handle *ce_usb_gethandle(int index); 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /sdk/include/celib_MadgwickAHRS.h: -------------------------------------------------------------------------------- 1 | //===================================================================================================== 2 | // MadgwickAHRS.h 3 | //===================================================================================================== 4 | // 5 | // Implementation of Madgwick's IMU and AHRS algorithms. 6 | // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 7 | // 8 | // Date Author Notes 9 | // 29/09/2011 SOH Madgwick Initial release 10 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 11 | // 12 | //===================================================================================================== 13 | #ifndef CELIB_MADGWICKAHRS_h 14 | #define CELIB_MADGWICKAHRS_h 15 | 16 | //---------------------------------------------------------------------------------------------------- 17 | // Variable declaration 18 | 19 | extern volatile float beta; // algorithm gain 20 | extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame 21 | 22 | //--------------------------------------------------------------------------------------------------- 23 | // Function declarations 24 | 25 | void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); 26 | void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az); 27 | 28 | #endif 29 | //===================================================================================================== 30 | // End of file 31 | //===================================================================================================== 32 | -------------------------------------------------------------------------------- /sdk/include/celib_img_process.h: -------------------------------------------------------------------------------- 1 | #ifndef CELIB_IMG_PROCESS_H 2 | #define CELIB_IMG_PROCESS_H 3 | 4 | 5 | 6 | void ce_merge_img(cv::Mat &dst,cv::Mat &src1,cv::Mat &src2); 7 | void ce_depth2color(cv::Mat & color, const cv::Mat & depth, const double max, const double min); 8 | 9 | 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /sdk/include/ceros_cam_d1_driver.h: -------------------------------------------------------------------------------- 1 | #ifndef CEROS_CAM_D1_DRIVER_H 2 | #define CEROS_CAM_D1_DRIVER_H 3 | 4 | #include "ros/ros.h" 5 | #include "std_msgs/String.h" 6 | #include 7 | #include 8 | #include "sensor_msgs/Imu.h" 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include "logmsg.h" 21 | #include "cpu_set.h" 22 | 23 | class CoolEyeCamD1 24 | { 25 | public: 26 | CoolEyeCamD1(ros::NodeHandle nh,char path[]); 27 | ~CoolEyeCamD1(); 28 | 29 | bool start_cam_D1_capture(); 30 | bool start_cam_D1_preprocess(); 31 | bool start_imu_icm26089(); 32 | 33 | void img_data_stream(); 34 | void imu_data_stream(); 35 | void exectu(); 36 | 37 | 38 | boost::thread *cam_thread_; 39 | boost::thread *imu_thread_; 40 | 41 | cv::Mat img_left_, img_right_, img_s1n0_, img_s1n1_, img_s1n2_; 42 | cv_bridge::CvImage left_bridge_, right_bridge_, s1n0_bridge_, s1n1_bridge_, s1n2_bridge_; 43 | 44 | image_transport::Publisher pub_caml_, pub_camr_, pub_cams1n0_, pub_cams1n1_, pub_cams1n2_; 45 | ros::Publisher pub_imu_; 46 | 47 | bool cam_thread_stop_run_; 48 | bool imu_thread_stop_run_; 49 | }; 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /sdk/include/cpu_set.h: -------------------------------------------------------------------------------- 1 | #ifndef _CPU_SET_H_ 2 | #define _CPU_SET_H_ 3 | 4 | #define CPU_SET_MAX 32 5 | 6 | class CCpuSet 7 | { 8 | public: 9 | static CCpuSet * & instance(); 10 | static void release(); 11 | 12 | int SetCpu(const char *pThreadName, int para = -1); 13 | int SetCpu(int nCpuId, const char *pThreadName, int para = -1); 14 | 15 | private: 16 | CCpuSet(); 17 | ~CCpuSet(); 18 | 19 | public: 20 | int m_nCpuSetFlag; 21 | int m_nCpuOccNum; 22 | int m_nCpuPoolSize; 23 | int m_aCpuPool[CPU_SET_MAX]; 24 | 25 | private: 26 | pthread_mutex_t m_Mutex; 27 | }; 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /sdk/include/logmsg.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _LOGMSG_HDR_ 3 | #define _LOGMSG_HDR_ 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | using std::vector; 12 | 13 | #define FILE_NAME_LENGTH_MAX 256 14 | #define SINGLE_LOG_SIZE_MAX 3072 15 | 16 | enum 17 | { 18 | LOGMSG_NULL = 0, 19 | LOGMSG_SHELL, 20 | LOGMSG_FILE, 21 | LOGMSG_ALL, 22 | }; 23 | 24 | 25 | enum 26 | { 27 | LOGMSG_LEVEL_NULL = 0, 28 | LOGMSG_LEVEL_DEBUG, 29 | LOGMSG_LEVEL_INFO, 30 | LOGMSG_LEVEL_WARN, 31 | LOGMSG_LEVEL_ERROR, 32 | }; 33 | 34 | 35 | struct LogFileInfo 36 | { 37 | char cName[FILE_NAME_LENGTH_MAX]; 38 | }; 39 | 40 | typedef vector vLogFile; 41 | 42 | class LogMsg 43 | { 44 | public: 45 | static LogMsg * & instance(); 46 | static void release(); 47 | 48 | int init(int nQuantity, int nSize, int nLevel = LOGMSG_LEVEL_ERROR); 49 | int fini(); 50 | int writeLog(int nLogFlag, int nLevel); 51 | const char * levelStr(int level); 52 | 53 | private: 54 | LogMsg(){}; 55 | virtual ~LogMsg(){}; 56 | 57 | public: 58 | char m_cBuff[SINGLE_LOG_SIZE_MAX]; 59 | unsigned int m_unLogIndex; 60 | 61 | private: 62 | int m_nMaxFileQuantity; 63 | int m_nMaxFileSize; 64 | int m_nLevel; 65 | FILE * m_pLog; 66 | time_t m_tTime; 67 | int m_nCount; 68 | char m_cPath[FILE_NAME_LENGTH_MAX]; 69 | char m_cFileName[FILE_NAME_LENGTH_MAX]; 70 | vLogFile m_vFileList; 71 | pthread_mutex_t m_Mutex; 72 | }; 73 | 74 | 75 | #define INIT_LOG(quantity, size, level) \ 76 | LogMsg::instance()->init(quantity, size, level) 77 | 78 | 79 | #define WRITE_LOG(logFlag, level,strFormat, args...) \ 80 | {\ 81 | time_t _t = time(NULL); \ 82 | tm _struTm; \ 83 | char _cTimeStr[64]; \ 84 | strftime( _cTimeStr, 64, "%Y/%m/%d %H:%M:%S", localtime_r(&_t, &_struTm) ); \ 85 | int _nBuffLen = snprintf(LogMsg::instance()->m_cBuff, SINGLE_LOG_SIZE_MAX-1, \ 86 | "[0x%04x]%-16s[%s]", LogMsg::instance()->m_unLogIndex, _cTimeStr, LogMsg::instance()->levelStr(level)); \ 87 | snprintf(&LogMsg::instance()->m_cBuff[_nBuffLen], SINGLE_LOG_SIZE_MAX-1-_nBuffLen, strFormat, ##args);\ 88 | LogMsg::instance()->writeLog(logFlag, level);\ 89 | } 90 | 91 | #define FINI_LOG() \ 92 | LogMsg::instance()->fini() 93 | 94 | #endif /*_LOGMSG_HDR_*/ 95 | -------------------------------------------------------------------------------- /sdk/include/threadsafe_queue.h: -------------------------------------------------------------------------------- 1 | #ifndef THREADSAFE_QUEUE_H 2 | #define THREADSAFE_QUEUE_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | /* 9 | * 线程安全队列 10 | * T为队列元素类型 11 | * 因为有std::mutex和std::condition_variable类成员,所以此类不支持复制构造函数也不支持赋值操作符(=) 12 | * */ 13 | 14 | /* 编译器支持c++11，经测试发现linux下gcc4.8.5支持，4.4.7不支持 */ 15 | //#define COMPILER_SUPPORTS_CPP11 16 | 17 | #define THREADSAFE_QUEUE_QUENTITY_MAX 10000 18 | 19 | template 20 | class threadsafe_queue{ 21 | private: 22 | 23 | //对列最大缓存数量 24 | unsigned int quentity_max; 25 | 26 | // data_queue访问信号量 27 | mutable std::mutex mut; 28 | mutable std::condition_variable data_cond; 29 | #ifdef COMPILER_SUPPORTS_CPP11 30 | using queue_type = std::queue; 31 | #else 32 | typedef std::queue queue_type; 33 | #endif 34 | queue_type data_queue; 35 | public: 36 | #ifdef COMPILER_SUPPORTS_CPP11 37 | using value_type= typename queue_type::value_type; 38 | using container_type = typename queue_type::container_type; 39 | #else 40 | typedef typename queue_type::value_type value_type; 41 | typedef typename queue_type::container_type container_type; 42 | #endif 43 | threadsafe_queue(){ 44 | quentity_max = THREADSAFE_QUEUE_QUENTITY_MAX; 45 | } 46 | 47 | threadsafe_queue(const threadsafe_queue&)=delete; 48 | threadsafe_queue& operator=(const threadsafe_queue&)=delete; 49 | 50 | /* 51 | * 使用迭代器为参数的构造函数,适用所有容器对象 52 | * */ 53 | template 54 | threadsafe_queue(_InputIterator first, _InputIterator last){ 55 | for(auto itor=first;itor!=last;++itor){ 56 | data_queue.push(*itor); 57 | } 58 | } 59 | 60 | explicit threadsafe_queue(const container_type &c):data_queue(c){} 61 | /* 62 | * 使用初始化列表为参数的构造函数 63 | * */ 64 | threadsafe_queue(std::initializer_list list):threadsafe_queue(list.begin(),list.end()){ 65 | } 66 | 67 | void init(unsigned int quentity){ 68 | quentity_max = quentity; 69 | } 70 | 71 | /* 72 | * 将元素加入队列，超过队列最大容量返回false 73 | * */ 74 | bool push(const value_type &new_value){ 75 | std::lock_guardlk(mut); 76 | if (data_queue.size() >= quentity_max) 77 | return false; 78 | 79 | data_queue.push(std::move(new_value)); 80 | data_cond.notify_one(); 81 | 82 | return true; 83 | } 84 | 85 | /* 86 | * 将元素加入队列，超过队列最大容量或者指定数量时，先移除一个元素再入队 87 | * */ 88 | int push(const value_type &new_value, value_type &old_value, unsigned int _quentity = THREADSAFE_QUEUE_QUENTITY_MAX){ 89 | int ret = 1; 90 | std::lock_guardlk(mut); 91 | 92 | unsigned int quentity = _quentity < quentity_max ? _quentity : quentity_max; 93 | if (data_queue.size() >= quentity && quentity > 0) 94 | { 95 | old_value=std::move(data_queue.front()); 96 | data_queue.pop(); 97 | ret = 0; 98 | } 99 | 100 | data_queue.push(std::move(new_value)); 101 | data_cond.notify_one(); 102 | return ret; 103 | } 104 | 105 | #ifdef COMPILER_SUPPORTS_CPP11 106 | /* 107 | * 从队列中弹出一个元素,如果队列为空就阻塞 108 | * */ 109 | value_type wait_and_pop(){ 110 | std::unique_locklk(mut); 111 | data_cond.wait(lk,[this]{return !this->data_queue.empty();}); 112 | auto value=std::move(data_queue.front()); 113 | data_queue.pop(); 114 | return value; 115 | } 116 | #endif 117 | /* 118 | * 从队列中弹出一个元素,如果队列为空返回false 119 | * */ 120 | bool try_pop(value_type& value){ 121 | std::lock_guardlk(mut); 122 | if(data_queue.empty()) 123 | return false; 124 | value=std::move(data_queue.front()); 125 | data_queue.pop(); 126 | return true; 127 | } 128 | 129 | /* 130 | * 从队列中取一个元素,但不出队,如果队列为空返回false 131 | * */ 132 | bool try_front(value_type& value){ 133 | 134 | std::lock_guardlk(mut); 135 | 136 | if(data_queue.empty()) 137 | 138 | return false; 139 | 140 | value=std::move(data_queue.front()); 141 | 142 | return true; 143 | 144 | } 145 | 146 | /* 147 | * 返回队列是否为空 148 | * */ 149 | auto empty() const->decltype(data_queue.empty()) { 150 | std::lock_guardlk(mut); 151 | return data_queue.empty(); 152 | } 153 | /* 154 | * 返回队列中元素数个 155 | * */ 156 | auto size() const->decltype(data_queue.size()){ 157 | std::lock_guardlk(mut); 158 | return data_queue.size(); 159 | } 160 | }; /* threadsafe_queue */ 161 | 162 | #endif /* COMMON_SOURCE_CPP_THREADSAFE_QUEUE_H_ */ 163 | -------------------------------------------------------------------------------- /sdk/rosfile/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cooleye_d1) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | sensor_msgs 10 | std_msgs 11 | cv_bridge 12 | image_transport 13 | message_generation 14 | message_filters 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | 19 | 20 | catkin_package( 21 | INCLUDE_DIRS include 22 | CATKIN_DEPENDS sensor_msgs std_msgs cv_bridge image_transport message_runtime 23 | # DEPENDS system_lib 24 | ) 25 | 26 | include_directories( 27 | include 28 | ${catkin_INCLUDE_DIRS} 29 | ) 30 | 31 | add_library(CELIB_BASE src/logmsg.cpp src/cpu_set.cpp) 32 | 33 | add_library(CEDRIVER_CONFIG src/cedriver_config.cpp) 34 | 35 | add_library(CEDRIVER_CAM src/cedriver_usb.cpp src/cedriver_cam.cpp) 36 | 37 | add_library(CELIB_MadgwickAHRS src/celib_MadgwickAHRS.cpp) 38 | 39 | add_library(CEDRIVER_IMU src/cedriver_imu.cpp ) 40 | target_link_libraries(CEDRIVER_IMU CELIB_MadgwickAHRS CELIB_BASE) 41 | 42 | add_library(CEROS_CAM_D1_DRIVER src/ceros_cam_d1_driver.cpp) 43 | target_link_libraries(CEROS_CAM_D1_DRIVER CEDRIVER_CONFIG CEDRIVER_CAM CEDRIVER_IMU CELIB_MadgwickAHRS CELIB_BASE usb-1.0 ${OpenCV_LIBS} ${catkin_LIBRARIES}) 44 | 45 | 46 | add_executable(CEROS_CAM_D1_NODE src/ceros_cam_d1_node.cpp) 47 | target_link_libraries(CEROS_CAM_D1_NODE CEROS_CAM_D1_DRIVER) 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /sdk/rosfile/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cooleye_d1 4 | 0.0.0 5 | The cooleye_d1 package 6 | 7 | cel 8 | 9 | TODO 10 | 11 | 12 | catkin 13 | roscpp 14 | sensor_msgs 15 | std_msgs 16 | cv_bridge 17 | image_transport 18 | message_generation 19 | message_filters 20 | 21 | roscpp 22 | sensor_msgs 23 | std_msgs 24 | cv_bridge 25 | image_transport 26 | message_runtime 27 | message_filters 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /sdk/src/ceapp_disp_filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "opencv2/core/core.hpp" 11 | #include "opencv2/imgproc/imgproc.hpp" 12 | #include "opencv2/highgui/highgui.hpp" 13 | #include "opencv2/calib3d/calib3d.hpp" 14 | 15 | #include "opencv2/core/utility.hpp" 16 | #include "opencv2/ximgproc/disparity_filter.hpp" 17 | 18 | #include "cedriver_usb.h" 19 | #include "cedriver_cam.h" 20 | #include "cedriver_imu.h" 21 | #include "cedriver_config.h" 22 | #include "cedriver_global_config.h" 23 | #include "celib_img_process.h" 24 | 25 | using namespace std; 26 | using namespace cv; 27 | using namespace cv::ximgproc; 28 | 29 | pthread_t ce_disp_filter_thread; 30 | bool ce_disp_filter_stop_run = false; 31 | 32 | bool g_bSaveImg = false; 33 | int g_nCtrl = 1; 34 | 35 | extern threadsafe_queue img_pkg_list_d1; 36 | 37 | 38 | 39 | Rect computeROI(Size2i src_sz, Ptr matcher_instance) 40 | { 41 | int min_disparity = matcher_instance->getMinDisparity(); 42 | int num_disparities = matcher_instance->getNumDisparities(); 43 | int block_size = matcher_instance->getBlockSize(); 44 | 45 | int bs2 = block_size/2; 46 | int minD = min_disparity, maxD = min_disparity + num_disparities - 1; 47 | 48 | int xmin = maxD + bs2; 49 | int xmax = src_sz.width + minD - bs2; 50 | int ymin = bs2; 51 | int ymax = src_sz.height - bs2; 52 | 53 | Rect r(xmin, ymin, xmax - xmin, ymax - ymin); 54 | return r; 55 | } 56 | 57 | int disp_filter(Mat& mat_Left, Mat& mat_Right, Mat& mat_raw_disp_vis, Mat& mat_filtered_disp_vis) 58 | { 59 | // parameters-init; 60 | bool no_downscale = true; 61 | int max_disp = 128; 62 | double lambda = 8000.0; 63 | double sigma = 1.5 ; 64 | double vis_mult = 1.0; 65 | int wsize = -1; 66 | 67 | String algo = "sgbm"; 68 | String filter = "wls_conf"; 69 | 70 | 71 | if(wsize<0) //user provided window_size value 72 | { 73 | if(algo=="sgbm") 74 | wsize = 3; //default window size for SGBM 75 | else if(!no_downscale && algo=="bm" && filter=="wls_conf") 76 | wsize = 7; //default window size for BM on downscaled views (downscaling is performed only for wls_conf) 77 | else 78 | wsize = 15; //default window size for BM on full-sized views 79 | } 80 | 81 | 82 | //! [load_views] 83 | Mat left = mat_Left; 84 | Mat right = mat_Right; 85 | 86 | //! [load_views] 87 | 88 | Mat left_for_matcher, right_for_matcher; 89 | Mat left_disp,right_disp; 90 | Mat filtered_disp; 91 | Mat conf_map = Mat(left.rows,left.cols,CV_8U); 92 | conf_map = Scalar(255); 93 | Rect ROI; 94 | Ptr wls_filter; 95 | double matching_time, filtering_time; 96 | 97 | if(max_disp<=0 || max_disp%16!=0) 98 | { 99 | cout<<"Incorrect max_disparity value: it should be positive and divisible by 16"; 100 | return -1; 101 | } 102 | if(wsize<=0 || wsize%2!=1) 103 | { 104 | cout<<"Incorrect window_size value: it should be positive and odd"; 105 | return -1; 106 | } 107 | 108 | if(filter=="wls_conf") // filtering with confidence (significantly better quality than wls_no_conf) 109 | { 110 | 111 | if(!no_downscale) 112 | { 113 | 114 | // downscale the views to speed-up the matching stage, as we will need to compute both left 115 | // and right disparity maps for confidence map computation 116 | //! [downscale] 117 | max_disp/=2; 118 | if(max_disp%16!=0) 119 | max_disp += 16-(max_disp%16); 120 | resize(left ,left_for_matcher ,Size(),0.5,0.5, INTER_LINEAR_EXACT); 121 | resize(right,right_for_matcher,Size(),0.5,0.5, INTER_LINEAR_EXACT); 122 | //! [downscale] 123 | } 124 | else 125 | { 126 | 127 | left_for_matcher = left.clone(); 128 | right_for_matcher = right.clone(); 129 | } 130 | 131 | 132 | if(algo=="bm") 133 | { 134 | //! [matching] 135 | Ptr left_matcher = StereoBM::create(max_disp,wsize); 136 | wls_filter = createDisparityWLSFilter(left_matcher); 137 | Ptr right_matcher = createRightMatcher(left_matcher); 138 | matching_time = (double)getTickCount(); 139 | left_matcher-> compute(left_for_matcher, right_for_matcher,left_disp); 140 | right_matcher->compute(right_for_matcher,left_for_matcher, right_disp); 141 | matching_time = ((double)getTickCount() - matching_time)/getTickFrequency(); 142 | //! [matching] 143 | 144 | } 145 | else if(algo=="sgbm") 146 | { 147 | Ptr left_matcher = StereoSGBM::create(0,max_disp,wsize); 148 | left_matcher->setP1(24*wsize*wsize); 149 | left_matcher->setP2(96*wsize*wsize); 150 | left_matcher->setPreFilterCap(63); 151 | left_matcher->setMode(StereoSGBM::MODE_SGBM_3WAY); 152 | wls_filter = createDisparityWLSFilter(left_matcher); 153 | Ptr right_matcher = createRightMatcher(left_matcher); 154 | 155 | matching_time = (double)getTickCount(); 156 | left_matcher-> compute(left_for_matcher, right_for_matcher,left_disp); 157 | right_matcher->compute(right_for_matcher,left_for_matcher, right_disp); 158 | matching_time = ((double)getTickCount() - matching_time)/getTickFrequency(); 159 | } 160 | else 161 | { 162 | cout<<"Unsupported algorithm"; 163 | return -1; 164 | } 165 | 166 | //! [filtering] 167 | wls_filter->setLambda(lambda); 168 | wls_filter->setSigmaColor(sigma); 169 | filtering_time = (double)getTickCount(); 170 | wls_filter->filter(left_disp,left,filtered_disp,right_disp); 171 | filtering_time = ((double)getTickCount() - filtering_time)/getTickFrequency(); 172 | //! [filtering] 173 | conf_map = wls_filter->getConfidenceMap(); 174 | 175 | // Get the ROI that was used in the last filter call: 176 | ROI = wls_filter->getROI(); 177 | if(!no_downscale) 178 | { 179 | // upscale raw disparity and ROI back for a proper comparison: 180 | resize(left_disp,left_disp,Size(),2.0,2.0,INTER_LINEAR_EXACT); 181 | left_disp = left_disp*2.0; 182 | ROI = Rect(ROI.x*2,ROI.y*2,ROI.width*2,ROI.height*2); 183 | } 184 | } 185 | else if(filter=="wls_no_conf") 186 | { 187 | /* There is no convenience function for the case of filtering with no confidence, so we 188 | will need to set the ROI and matcher parameters manually */ 189 | 190 | left_for_matcher = left.clone(); 191 | right_for_matcher = right.clone(); 192 | 193 | if(algo=="bm") 194 | { 195 | Ptr matcher = StereoBM::create(max_disp,wsize); 196 | matcher->setTextureThreshold(0); 197 | matcher->setUniquenessRatio(0); 198 | 199 | ROI = computeROI(left_for_matcher.size(),matcher); 200 | wls_filter = createDisparityWLSFilterGeneric(false); 201 | wls_filter->setDepthDiscontinuityRadius((int)ceil(0.33*wsize)); 202 | 203 | matching_time = (double)getTickCount(); 204 | matcher->compute(left_for_matcher,right_for_matcher,left_disp); 205 | matching_time = ((double)getTickCount() - matching_time)/getTickFrequency(); 206 | } 207 | else if(algo=="sgbm") 208 | { 209 | Ptr matcher = StereoSGBM::create(0,max_disp,wsize); 210 | matcher->setUniquenessRatio(0); 211 | matcher->setDisp12MaxDiff(1000000); 212 | matcher->setSpeckleWindowSize(0); 213 | matcher->setP1(24*wsize*wsize); 214 | matcher->setP2(96*wsize*wsize); 215 | matcher->setMode(StereoSGBM::MODE_SGBM_3WAY); 216 | ROI = computeROI(left_for_matcher.size(),matcher); 217 | wls_filter = createDisparityWLSFilterGeneric(false); 218 | wls_filter->setDepthDiscontinuityRadius((int)ceil(0.5*wsize)); 219 | 220 | matching_time = (double)getTickCount(); 221 | matcher->compute(left_for_matcher,right_for_matcher,left_disp); 222 | matching_time = ((double)getTickCount() - matching_time)/getTickFrequency(); 223 | } 224 | else 225 | { 226 | cout<<"Unsupported algorithm"; 227 | return -1; 228 | } 229 | 230 | wls_filter->setLambda(lambda); 231 | wls_filter->setSigmaColor(sigma); 232 | filtering_time = (double)getTickCount(); 233 | wls_filter->filter(left_disp,left,filtered_disp,Mat(),ROI); 234 | filtering_time = ((double)getTickCount() - filtering_time)/getTickFrequency(); 235 | } 236 | else 237 | { 238 | cout<<"Unsupported filter"; 239 | return -1; 240 | } 241 | 242 | 243 | //collect and print all the stats: 244 | cout.precision(2); 245 | cout<<"Matching time: "<left_img; 286 | delete img_lr_pkg->right_img; 287 | delete img_lr_pkg; 288 | continue; 289 | } 290 | 291 | // get the image 292 | memcpy(img_left.data, img_lr_pkg->left_img->data, ce_config_get_cf_img_size()); 293 | memcpy(img_right.data,img_lr_pkg->right_img->data,ce_config_get_cf_img_size()); 294 | 295 | 296 | disp_filter(img_left, 297 | img_right, 298 | disp8_raw, 299 | disp8_filter); 300 | 301 | ce_depth2color(disRGB_raw, disp8_raw, 255, 0); 302 | ce_depth2color(disRGB_filter, disp8_filter, 255, 0); 303 | 304 | 305 | ce_merge_img(result_raw,disp8_raw,disp8_filter); 306 | ce_merge_img(result_filter,disRGB_raw,disRGB_filter); 307 | 308 | cv::imshow("result_raw",result_raw); 309 | cv::imshow("result_filter",result_filter); 310 | 311 | //reprojectImageTo3D(disp, xyz, Q, true); 312 | //imshow("xyz", xyz); 313 | 314 | cv::waitKey(1); 315 | 316 | delete img_lr_pkg->left_img; 317 | delete img_lr_pkg->right_img; 318 | delete img_lr_pkg; 319 | } 320 | ce_disp_filter_thread = 0; 321 | pthread_exit(NULL); 322 | } 323 | 324 | 325 | int ce_disp_filter_init() 326 | { 327 | int temp = pthread_create(&ce_disp_filter_thread, NULL, ce_disp_filter, NULL); 328 | if(temp) 329 | { 330 | printf("celog: Failed to create thread show image \r\n"); 331 | return ERROR; 332 | } 333 | return SUCCESS; 334 | } 335 | 336 | void ce_disp_filter_close() 337 | { 338 | ce_disp_filter_stop_run = true; 339 | if(ce_disp_filter_thread != 0) 340 | { 341 | pthread_join(ce_disp_filter_thread,NULL); 342 | } 343 | } 344 | 345 | 346 | void SIGINTHandler(int nSig) 347 | { 348 | printf("capture a SIGINT signal %d\n", nSig); 349 | if(0 != g_nCtrl) 350 | g_nCtrl = 0; 351 | else 352 | g_nCtrl = 1; 353 | } 354 | 355 | 356 | 357 | int main(int argc, char* argv[]) 358 | { 359 | signal(SIGINT, SIGINTHandler); 360 | 361 | ce_config_load_settings("../config/cecfg_std.txt"); 362 | 363 | int fd = ce_imu_capture_init(); 364 | if(fd < 0) 365 | { 366 | printf("celog: imu caputre error\r\n"); 367 | } 368 | 369 | else 370 | { 371 | printf("celog: imu caputre success\r\n"); 372 | 373 | fd = ce_imu_showdata_init(); 374 | if(fd < 0) 375 | printf("celog: imu show data error\r\n"); 376 | else 377 | printf("celog: imu show data success\r\n"); 378 | } 379 | 380 | 381 | int r = ce_cam_capture_init(); 382 | if(r < 0) 383 | { 384 | printf("celog: cam capture error \r\n"); 385 | } 386 | else 387 | { 388 | printf("celog: cam capture success \r\n"); 389 | 390 | r = ce_cam_preprocess_init(); 391 | if(r < 0) 392 | { 393 | printf("celog: cam preprocess error \r\n"); 394 | } 395 | else 396 | { 397 | printf("celog: cam preprocess sucess \r\n"); 398 | 399 | r = ce_disp_filter_init(); 400 | if(r < 0) 401 | printf("celog: disp_filter image error \r\n"); 402 | else 403 | printf("celog: disp_filter image sucess \r\n"); 404 | } 405 | } 406 | 407 | g_nCtrl = 1; 408 | 409 | while(g_nCtrl) 410 | { 411 | sleep(100); 412 | } 413 | 414 | ce_imu_capture_close(); 415 | ce_imu_showdata_close(); 416 | 417 | ce_cam_capture_close(); 418 | ce_disp_filter_close(); 419 | return 0; 420 | } 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | -------------------------------------------------------------------------------- /sdk/src/ceapp_raw_data.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | #include 14 | #include 15 | 16 | #include "cedriver_usb.h" 17 | #include "cedriver_cam.h" 18 | #include "cedriver_imu.h" 19 | #include "cedriver_config.h" 20 | #include "logmsg.h" 21 | #include "cpu_set.h" 22 | 23 | int g_nCtrl = 1; 24 | extern int g_imgvb; 25 | 26 | #define CE_RAW_DATA_IMU 27 | 28 | void SIGINTHandler(int nSig) 29 | { 30 | g_nCtrl = 0; 31 | printf("SIGINTHandler: g_nCtrl = %d\r\n", g_nCtrl); 32 | } 33 | 34 | int main(int argc, char* argv[]) 35 | { 36 | int r = 0; 37 | signal(SIGINT, SIGINTHandler); 38 | INIT_LOG(1000, 1000, LOGMSG_LEVEL_DEBUG); 39 | 40 | if (argc == 2) 41 | { 42 | g_imgvb = atoi(argv[1]); 43 | } 44 | 45 | int nprocs = get_nprocs(); 46 | if (0 == nprocs || 1 == nprocs) 47 | { 48 | CCpuSet::instance()->m_nCpuSetFlag = false; 49 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "The number of cores is %d, and the cores are not bound\r\n", nprocs); 50 | } 51 | else 52 | { 53 | CCpuSet::instance()->m_nCpuSetFlag = true; 54 | for (int i = 0; i < nprocs - 1; i++) 55 | { 56 | if(i >= CPU_SET_MAX) 57 | break; 58 | 59 | CCpuSet::instance()->m_aCpuPool[i] = i + 1; 60 | CCpuSet::instance()->m_nCpuPoolSize++; 61 | } 62 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, 63 | "The number of cores is %d. The binding cores are %d to %d. Core 0 is used for special purposes.\r\n", 64 | nprocs, 1, CCpuSet::instance()->m_nCpuPoolSize); 65 | } 66 | 67 | ce_config_load_settings("../config/cecfg_std.txt"); 68 | 69 | #ifdef CE_RAW_DATA_IMU 70 | int fd = ce_imu_capture_init(); 71 | if(fd < 0) 72 | { 73 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_ERROR, "ce_imu_capture_init error\r\n"); 74 | } 75 | 76 | else 77 | { 78 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "ce_imu_capture_init success\r\n"); 79 | 80 | fd = ce_imu_showdata_init(); 81 | if(fd < 0) 82 | { 83 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_ERROR, "ce_imu_showdata_init error\r\n"); 84 | } 85 | else 86 | { 87 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "ce_imu_showdata_init success\r\n"); 88 | } 89 | } 90 | #endif 91 | 92 | r = ce_cam_preprocess_init(); 93 | if(r < 0) 94 | { 95 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_ERROR, "ce_cam_preprocess_init error \r\n"); 96 | } 97 | else 98 | { 99 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "ce_cam_preprocess_init sucess \r\n"); 100 | 101 | r = ce_cam_showimg_init(); 102 | if(r < 0) 103 | { 104 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_ERROR, "ce_cam_showimg_init error \r\n"); 105 | } 106 | else 107 | { 108 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "ce_cam_showimg_init sucess \r\n"); 109 | } 110 | } 111 | 112 | g_nCtrl = 1; 113 | r = ce_cam_capture_init(); 114 | if(r < 0) 115 | { 116 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_ERROR, "ce_cam_capture_init error \r\n"); 117 | } 118 | else 119 | { 120 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "ce_cam_capture_init success \r\n"); 121 | } 122 | 123 | while(g_nCtrl) 124 | { 125 | sleep(1); 126 | } 127 | 128 | #ifdef CE_RAW_DATA_IMU 129 | ce_imu_capture_close(); 130 | ce_imu_showdata_close(); 131 | #endif 132 | 133 | ce_cam_showimg_close(); 134 | usleep(10000); 135 | 136 | FINI_LOG(); 137 | return 0; 138 | } 139 | -------------------------------------------------------------------------------- /sdk/src/ceapp_stereo_match.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | 12 | using namespace std; 13 | 14 | 15 | #include "opencv2/core/core.hpp" 16 | #include "opencv2/imgproc/imgproc.hpp" 17 | #include "opencv2/highgui/highgui.hpp" 18 | #include "opencv2/calib3d/calib3d.hpp" 19 | 20 | #include "opencv2/core/utility.hpp" 21 | #include "opencv2/ximgproc/disparity_filter.hpp" 22 | 23 | #include "cedriver_usb.h" 24 | #include "cedriver_cam.h" 25 | #include "cedriver_imu.h" 26 | #include "cedriver_config.h" 27 | #include "cedriver_global_config.h" 28 | #include "celib_img_process.h" 29 | 30 | 31 | 32 | using namespace cv; 33 | 34 | pthread_t ce_ste_match_thread; 35 | bool ce_ste_match_stop_run = false; 36 | 37 | 38 | bool g_bSaveImg = false; 39 | 40 | extern threadsafe_queue img_pkg_list_d1; 41 | 42 | int g_nCtrl = 1; 43 | 44 | 45 | 46 | static void* ce_ste_match(void *) 47 | { 48 | ce_config_get_cf_cam_rectify_force_on(); 49 | 50 | cv::Mat img_left(cv::Size(ce_config_get_cf_img_width(),ce_config_get_cf_img_height()),CV_8UC1); 51 | cv::Mat img_right(cv::Size(ce_config_get_cf_img_width(),ce_config_get_cf_img_height()),CV_8UC1); 52 | 53 | cv::Mat disparity(cv::Size(ce_config_get_cf_img_width(),ce_config_get_cf_img_height()),CV_8UC1); 54 | cv::Mat result_rect; 55 | 56 | Mat Q; 57 | Mat xyz; 58 | Mat disp, disp8; 59 | Rect roi1, roi2; 60 | 61 | Mat disRGB; 62 | 63 | Ptr bm = StereoBM::create(16,9); 64 | Ptr sgbm = StereoSGBM::create(0,16,3); 65 | 66 | //////////////////////////////////////////////////////////////////////////////////// 67 | enum { STEREO_BM=0, STEREO_SGBM=1, STEREO_HH=2, STEREO_VAR=3, STEREO_3WAY=4 }; 68 | int alg = STEREO_SGBM; 69 | int SADWindowSize, numberOfDisparities; 70 | bool no_display; 71 | float scale; 72 | 73 | 74 | alg = ce_config_get_cf_ste_algorithm(); 75 | 76 | 77 | numberOfDisparities = 128; 78 | SADWindowSize =15; 79 | scale = 1.0; 80 | no_display = false; 81 | 82 | int color_mode = alg == STEREO_BM ? 0 : -1; 83 | 84 | Mat img1 = img_left; 85 | Mat img2 = img_right; 86 | 87 | Size img_size = img1.size(); 88 | 89 | numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width/8) + 15) & -16; 90 | 91 | bm->setROI1(roi1); 92 | bm->setROI2(roi2); 93 | bm->setPreFilterCap(31); 94 | bm->setBlockSize(SADWindowSize > 0 ? SADWindowSize : 9); 95 | bm->setMinDisparity(0); 96 | bm->setNumDisparities(numberOfDisparities); 97 | bm->setTextureThreshold(10); 98 | bm->setUniquenessRatio(15); 99 | bm->setSpeckleWindowSize(100); 100 | bm->setSpeckleRange(32); 101 | bm->setDisp12MaxDiff(1); 102 | 103 | sgbm->setPreFilterCap(63); 104 | int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3; 105 | sgbm->setBlockSize(sgbmWinSize); 106 | 107 | int cn = img1.channels(); 108 | 109 | sgbm->setP1(8*cn*sgbmWinSize*sgbmWinSize); 110 | sgbm->setP2(32*cn*sgbmWinSize*sgbmWinSize); 111 | sgbm->setMinDisparity(0); 112 | sgbm->setNumDisparities(numberOfDisparities); 113 | sgbm->setUniquenessRatio(10); 114 | sgbm->setSpeckleWindowSize(100); 115 | sgbm->setSpeckleRange(32); 116 | sgbm->setDisp12MaxDiff(1); 117 | if(alg==STEREO_HH) 118 | sgbm->setMode(StereoSGBM::MODE_HH); 119 | else if(alg==STEREO_SGBM) 120 | sgbm->setMode(StereoSGBM::MODE_SGBM); 121 | else if(alg==STEREO_3WAY) 122 | sgbm->setMode(StereoSGBM::MODE_SGBM_3WAY); 123 | 124 | /////////////////////////////////////////////////////////// 125 | 126 | 127 | d1_img_output_pkg *img_lr_pkg; 128 | 129 | while(!ce_ste_match_stop_run) 130 | { 131 | if(!img_pkg_list_d1.try_pop(img_lr_pkg)) 132 | { 133 | usleep(1000); 134 | continue; 135 | } 136 | 137 | if(img_pkg_list_d1.size() != 0) 138 | { 139 | delete img_lr_pkg->left_img; 140 | delete img_lr_pkg->right_img; 141 | delete img_lr_pkg; 142 | continue; 143 | } 144 | 145 | // get the image 146 | memcpy(img_left.data, img_lr_pkg->left_img->data, ce_config_get_cf_img_size()); 147 | memcpy(img_right.data,img_lr_pkg->right_img->data,ce_config_get_cf_img_size()); 148 | 149 | 150 | img1 = img_left; 151 | img2 = img_right; 152 | 153 | int64 t = getTickCount(); 154 | if( alg == STEREO_BM ) 155 | bm->compute(img1, img2, disp); 156 | else if( alg == STEREO_SGBM || alg == STEREO_HH || alg == STEREO_3WAY ) 157 | sgbm->compute(img1, img2, disp); 158 | 159 | t = getTickCount() - t; 160 | printf("Time elapsed: %fms\n", t*1000/getTickFrequency()); 161 | 162 | //disp = dispp.colRange(numberOfDisparities, img1p.cols); 163 | if( alg != STEREO_VAR ) 164 | disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.)); 165 | else 166 | disp.convertTo(disp8, CV_8U); 167 | 168 | //reprojectImageTo3D(disp, xyz, Q, true); 169 | //imshow("xyz", xyz); 170 | 171 | ce_merge_img(result_rect, img_left, img_right); 172 | 173 | cv::imshow("result_rect",result_rect); 174 | 175 | imshow("disparity", disp8); 176 | ce_depth2color(disRGB, disp8, 255, 0); 177 | imshow("disRGB", disRGB); 178 | 179 | cv::waitKey(1); 180 | 181 | delete img_lr_pkg->left_img; 182 | delete img_lr_pkg->right_img; 183 | delete img_lr_pkg; 184 | } 185 | ce_ste_match_thread = 0; 186 | pthread_exit(NULL); 187 | } 188 | 189 | 190 | int ce_ste_match_init() 191 | { 192 | int temp = pthread_create(&ce_ste_match_thread, NULL, ce_ste_match, NULL); 193 | if(temp) 194 | { 195 | printf("celog: Failed to create thread show image \r\n"); 196 | return ERROR; 197 | } 198 | return SUCCESS; 199 | } 200 | 201 | void ce_ste_match_close() 202 | { 203 | ce_ste_match_stop_run = true; 204 | if(ce_ste_match_thread != 0) 205 | { 206 | pthread_join(ce_ste_match_thread,NULL); 207 | } 208 | } 209 | 210 | 211 | 212 | 213 | 214 | void SIGINTHandler(int nSig) 215 | { 216 | printf("capture a SIGINT signal %d\n", nSig); 217 | if(0 != g_nCtrl) 218 | g_nCtrl = 0; 219 | else 220 | g_nCtrl = 1; 221 | } 222 | 223 | 224 | int main(int argc, char* argv[]) 225 | { 226 | signal(SIGINT, SIGINTHandler); 227 | g_nCtrl = 1; 228 | ce_config_load_settings("../config/cecfg_std.txt"); 229 | 230 | int fd = ce_imu_capture_init(); 231 | if(fd < 0) 232 | { 233 | printf("celog: imu caputre error\r\n"); 234 | } 235 | 236 | else 237 | { 238 | printf("celog: imu caputre success\r\n"); 239 | 240 | fd = ce_imu_showdata_init(); 241 | if(fd < 0) 242 | printf("celog: imu show data error\r\n"); 243 | else 244 | printf("celog: imu show data success\r\n"); 245 | } 246 | 247 | 248 | int r = ce_cam_capture_init(); 249 | if(r < 0) 250 | { 251 | printf("celog: cam capture error \r\n"); 252 | } 253 | else 254 | { 255 | printf("celog: cam capture success \r\n"); 256 | 257 | r = ce_cam_preprocess_init(); 258 | if(r < 0) 259 | { 260 | printf("celog: cam preprocess error \r\n"); 261 | } 262 | else 263 | { 264 | printf("celog: cam preprocess sucess \r\n"); 265 | 266 | r = ce_ste_match_init(); 267 | if(r < 0) 268 | printf("celog: ste_match image error \r\n"); 269 | else 270 | printf("celog: ste_match image sucess \r\n"); 271 | } 272 | } 273 | 274 | while(g_nCtrl) 275 | { 276 | sleep(100); 277 | } 278 | 279 | ce_imu_capture_close(); 280 | 281 | ce_imu_showdata_close(); 282 | 283 | ce_cam_capture_close(); 284 | 285 | ce_ste_match_close(); 286 | return 0; 287 | } 288 | 289 | 290 | -------------------------------------------------------------------------------- /sdk/src/ceapp_write_deviceid.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include "logmsg.h" 16 | #include "cedriver_usb.h" 17 | #include "cedriver_cam.h" 18 | #include "cedriver_imu.h" 19 | #include "cedriver_config.h" 20 | 21 | #define LOG printf 22 | #define CE_CAM_SERIAL_FILE_NAME "./serial.log" 23 | 24 | int g_nCtrl = 0; 25 | char g_cCamType = CE_CAM_TYPE_D; 26 | extern int g_nForceWriteFlag; 27 | 28 | extern int openStdoutFile(); 29 | extern int closeStdoutFile(); 30 | 31 | void SIGINTHandler(int nSig) 32 | { 33 | if(0 != g_nCtrl) 34 | g_nCtrl = 0; 35 | else 36 | g_nCtrl = 1; 37 | } 38 | 39 | int main(int argc, char* argv[]) 40 | { 41 | int r = 0; 42 | signal(SIGINT, SIGINTHandler); 43 | 44 | INIT_LOG(1000, 1000, LOGMSG_LEVEL_DEBUG); 45 | 46 | g_nForceWriteFlag = 0; 47 | g_cCamType = CE_CAM_TYPE_D; 48 | 49 | for (int i = 1; i < argc; i++) 50 | { 51 | if (argv[i][0] == '-') 52 | { 53 | switch(argv[i][1]) 54 | { 55 | case 'f': 56 | g_nForceWriteFlag = 1; 57 | break; 58 | case 'S': 59 | g_cCamType = CE_CAM_TYPE_S; 60 | break; 61 | case 'D': 62 | g_cCamType = CE_CAM_TYPE_D; 63 | break; 64 | default: 65 | break; 66 | } 67 | } 68 | 69 | } 70 | 71 | openStdoutFile(); 72 | //ce_config_load_settings("../config/cecfg_std.txt"); 73 | 74 | printf("\r\n"); 75 | if (CE_CAM_TYPE_S == g_cCamType) 76 | { 77 | if (1 == g_nForceWriteFlag) 78 | printf("################ \033[1m\033[45;33m单目\033[0m摄像头\033[1m\033[45;33m强制\033[0m烧写模式 ################\r\n\r\n"); 79 | else 80 | printf("################ \033[1m\033[45;33m单目\033[0m摄像头\033[1m\033[45;33m普通\033[0m烧写模式 ################\r\n\r\n"); 81 | 82 | r = ce_cam_write_deviceid_S1(); 83 | } 84 | else 85 | { 86 | if (1 == g_nForceWriteFlag) 87 | printf("################ \033[1m\033[45;33m双目\033[0m摄像头\033[1m\033[45;33m强制\033[0m烧写模式 ################\r\n\r\n"); 88 | else 89 | printf("################ \033[1m\033[45;33m双目\033[0m摄像头\033[1m\033[45;33m普通\033[0m烧写模式 ################\r\n\r\n"); 90 | 91 | r = ce_cam_write_deviceid_D1(); 92 | } 93 | 94 | if(r < 0) 95 | { 96 | printf("celog: cam capture error \r\n"); 97 | } 98 | else 99 | { 100 | printf("celog: cam capture success \r\n"); 101 | } 102 | 103 | closeStdoutFile(); 104 | 105 | FINI_LOG(); 106 | return 0; 107 | } 108 | -------------------------------------------------------------------------------- /sdk/src/cedriver_config.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "cedriver_config.h" 8 | #include "cedriver_global_config.h" 9 | #include 10 | 11 | global_config_d1 gc_camd1; 12 | int g_imgvb = 0; 13 | 14 | std::ofstream g_PrintfFile; 15 | 16 | #if 1 17 | #define CE_COUT std::cout 18 | #else 19 | #define CE_COUT g_PrintfFile 20 | #endif 21 | 22 | int openStdoutFile() 23 | { 24 | 25 | g_PrintfFile.open("./printf.log"); 26 | assert(g_PrintfFile.is_open()); 27 | } 28 | 29 | void closeStdoutFile() 30 | { 31 | g_PrintfFile.close(); //关闭文件输入流 32 | } 33 | 34 | 35 | static std::string ce_config_get_cf_string_para(std::vector *tconfiglist,std::string cf_name) 36 | { 37 | std::string stemp; 38 | 39 | for(int i=0; i< tconfiglist->size(); i++) 40 | { 41 | stemp = tconfiglist->at(i).substr(0, tconfiglist->at(i).find("=")); 42 | if(!stemp.compare(cf_name)) 43 | { 44 | return tconfiglist->at(i).substr(tconfiglist->at(i).find("=")+1, tconfiglist->at(i).find(";") - tconfiglist->at(i).find("=") -1); 45 | } 46 | } 47 | 48 | return "GET DEVICE STRING ERROR!!!"; 49 | } 50 | 51 | static int ce_config_get_cf_int_para(std::vector *tconfiglist, std::string cf_name) 52 | { 53 | std::string stemp; 54 | std::string scf = "ERROR"; 55 | 56 | for(int i=0; i< tconfiglist->size(); i++) 57 | { 58 | stemp = tconfiglist->at(i).substr(0, tconfiglist->at(i).find("=")); 59 | if(!stemp.compare(cf_name)) 60 | { 61 | scf = tconfiglist->at(i).substr(tconfiglist->at(i).find("=")+1, tconfiglist->at(i).find(";") - tconfiglist->at(i).find("=") -1); 62 | break; 63 | } 64 | } 65 | 66 | if(scf == "ERROR") 67 | { 68 | return ERROR; 69 | } 70 | else 71 | { 72 | return strtol(scf.c_str(),NULL,10); 73 | } 74 | 75 | } 76 | 77 | static float ce_config_get_cf_float_para(std::vector *tconfiglist, std::string cf_name) 78 | { 79 | std::string stemp; 80 | std::string scf = "ERROR"; 81 | 82 | for(int i=0; i< tconfiglist->size(); i++) 83 | { 84 | stemp = tconfiglist->at(i).substr(0, tconfiglist->at(i).find("=")); 85 | if(!stemp.compare(cf_name)) 86 | { 87 | scf = tconfiglist->at(i).substr(tconfiglist->at(i).find("=")+1, tconfiglist->at(i).find(";") - tconfiglist->at(i).find("=") -1); 88 | break; 89 | } 90 | } 91 | 92 | if(scf == "ERROR") 93 | { 94 | return ERROR; 95 | } 96 | else 97 | { 98 | return strtof(scf.c_str(),NULL); 99 | } 100 | 101 | } 102 | 103 | 104 | static void ce_config_set_cf_float_para(std::string cecamd1_config_file, std::string cf_name,float pdata) 105 | { 106 | 107 | std::ifstream in_settings(cecamd1_config_file.c_str()); 108 | if(!in_settings ) 109 | { 110 | std::cerr << "celog: opening file failed -- "< settingsList; 118 | int index = 0; 119 | 120 | char temp[30] = {0}; 121 | sprintf(temp,"%f",pdata); 122 | std::string scf = temp; 123 | 124 | std::string line_settings; 125 | while(getline(in_settings,line_settings)) 126 | { 127 | std::string stemp = line_settings.substr(0, line_settings.find("=")); 128 | if(!stemp.compare(cf_name)) 129 | { 130 | line_settings.clear(); 131 | line_settings = cf_name + "=" + scf + ";"; 132 | index = settingsList.size(); 133 | } 134 | 135 | settingsList.push_back(line_settings); 136 | line_settings.clear(); 137 | } 138 | in_settings.close(); 139 | 140 | std::ofstream out_settings(cecamd1_config_file.c_str()); 141 | for (std::vector::iterator iter=settingsList.begin();iter!=settingsList.end();iter++) 142 | { 143 | out_settings << *iter << std::endl; 144 | } 145 | out_settings.close(); 146 | } 147 | 148 | 149 | static void ce_config_get_img_config() 150 | { 151 | switch(gc_camd1.gc_cam.cf_cam_resolution) 152 | { 153 | case CAMD1_RESOLUTION_VGA : 154 | gc_camd1.gc_img.cf_img_width = IMG_WIDTH_VGA; 155 | gc_camd1.gc_img.cf_img_height = IMG_HEIGHT_VGA; 156 | break; 157 | case CAMD1_RESOLUTION_WVGA : 158 | gc_camd1.gc_img.cf_img_width = IMG_WIDTH_WVGA; 159 | gc_camd1.gc_img.cf_img_height = IMG_HEIGHT_WVGA; 160 | break; 161 | default : 162 | gc_camd1.gc_img.cf_img_width = IMG_WIDTH_WVGA; 163 | gc_camd1.gc_img.cf_img_height = IMG_HEIGHT_WVGA; 164 | break; 165 | } 166 | 167 | gc_camd1.gc_img.cf_img_size = gc_camd1.gc_img.cf_img_width * gc_camd1.gc_img.cf_img_height; 168 | gc_camd1.gc_img.cf_img_buff_size = gc_camd1.gc_img.cf_img_size + 0x200; 169 | 170 | /////////HS--VS/////////////////////////////// 171 | if(gc_camd1.gc_cam.cf_cam_FPS < 10) 172 | { 173 | gc_camd1.gc_cam.cf_cam_FPS = 10; 174 | gc_camd1.gc_img.cf_img_HB = 0x03FF; 175 | } 176 | else if(gc_camd1.gc_cam.cf_cam_FPS < 31) 177 | gc_camd1.gc_img.cf_img_HB = 0x03FF; 178 | else if(gc_camd1.gc_cam.cf_cam_FPS < 41) 179 | gc_camd1.gc_img.cf_img_HB = 0x01FF; 180 | else if(gc_camd1.gc_cam.cf_cam_FPS < 45) 181 | gc_camd1.gc_img.cf_img_HB = 0x0160; 182 | else 183 | { 184 | gc_camd1.gc_cam.cf_cam_FPS = 45; 185 | gc_camd1.gc_img.cf_img_HB = 0x0160; 186 | } 187 | 188 | 189 | // WIDTH *(A + Q) + REG06(A + Q) +4 = 1/FPS 190 | // A = cf_img_width Q = cf_img_VB = reg05 191 | gc_camd1.gc_img.cf_img_VB = (CAMD1_SYS_CLKIN/gc_camd1.gc_cam.cf_cam_FPS - 4)/(gc_camd1.gc_img.cf_img_width + gc_camd1.gc_img.cf_img_HB) - gc_camd1.gc_img.cf_img_height; 192 | std::cout << "cf_img_VB_old: " << gc_camd1.gc_img.cf_img_VB < settingsList; 254 | int linecount=0; 255 | 256 | while(getline(ifs_settings,line_settings)) 257 | { 258 | if(line_settings.at(0)!='#') 259 | { 260 | settingsList.push_back(line_settings); 261 | linecount++; 262 | } 263 | } 264 | ifs_settings.close(); 265 | 266 | 267 | 268 | char fileFullName1[256] = {0}; 269 | char fileFullName2[256] = {0}; 270 | 271 | char *pCur = (char*)settings_file; 272 | char *pEnd = pCur + strlen(settings_file); 273 | while(pCur < pEnd) 274 | { 275 | if (*pEnd == '/') 276 | break; 277 | pEnd--; 278 | } 279 | 280 | if (*pEnd == '/') 281 | { 282 | pEnd++; 283 | memcpy(fileFullName1, pCur, pEnd - pCur); 284 | memcpy(fileFullName2, pCur, pEnd - pCur); 285 | } 286 | 287 | strcat(fileFullName1, "intrinsics.yml"); 288 | strcat(fileFullName2, "extrinsics.yml"); 289 | 290 | gc_camd1.gc_cam.cf_cam_intrinsics = fileFullName1; 291 | gc_camd1.gc_cam.cf_cam_extrinsics = fileFullName2; 292 | 293 | 294 | gc_camd1.gc_dev.cf_dev_name = ce_config_get_cf_string_para(&settingsList, "cf_dev_name"); 295 | gc_camd1.gc_dev.cf_dev_version = ce_config_get_cf_string_para(&settingsList, "cf_dev_version"); 296 | 297 | CE_COUT << "cf_dev_name: "<< gc_camd1.gc_dev.cf_dev_name < 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "cedriver_imu.h" 22 | #include "cedriver_global_config.h" 23 | #include "threadsafe_queue.h" 24 | #include "cedriver_config.h" 25 | #include "celib_MadgwickAHRS.h" 26 | #include "logmsg.h" 27 | #include "cpu_set.h" 28 | 29 | threadsafe_queue icm20689_pkg_list; 30 | 31 | pthread_t ce_imu_capture_thread; 32 | bool ce_imu_capture_stop_run = false; 33 | 34 | pthread_t ce_imu_showdata_thread; 35 | bool ce_imu_showdata_stop_run = false; 36 | 37 | 38 | int ce_imu_uart_fd = 0; 39 | 40 | static int ce_imu_find_pkg_head(unsigned char* buf,int len) 41 | { 42 | int i; 43 | for(i=0; inum = imu_frame[2]; 197 | 198 | float fgyro_nobias[3]= { 199 | ((float)imu_data[0]-gyro_offset[0]), 200 | ((float)imu_data[1]-gyro_offset[1]), 201 | ((float)imu_data[2]-gyro_offset[2])}; 202 | 203 | ticm20689_pkg->rx = 1.0*fgyro_nobias[0]/32768*500; 204 | ticm20689_pkg->ry = 1.0*fgyro_nobias[1]/32768*500; 205 | ticm20689_pkg->rz = 1.0*fgyro_nobias[2]/32768*500; 206 | 207 | 208 | 209 | float facc_nobias[3]= { 210 | ((float)imu_data[3]-acc_offset[0]), 211 | ((float)imu_data[4]-acc_offset[1]), 212 | ((float)imu_data[5]-acc_offset[2])}; 213 | 214 | 215 | 216 | float facc_t[3] ={0,0,0}; 217 | 218 | facc_t[0] = 1.0*facc_nobias[0]/8192*STD_g; 219 | facc_t[1] = 1.0*facc_nobias[1]/8192*STD_g; 220 | facc_t[2] = 1.0*facc_nobias[2]/8192*STD_g; 221 | 222 | // ticm20689_pkg->ax = 1.0*facc_nobias[0]/8192*STD_g; 223 | // ticm20689_pkg->ay = 1.0*facc_nobias[1]/8192*STD_g; 224 | // ticm20689_pkg->az = 1.0*facc_nobias[2]/8192*STD_g; 225 | 226 | ticm20689_pkg->ax = RotMat[0][0]*facc_t[0] + RotMat[0][1]*facc_t[1] + RotMat[0][2]*facc_t[2]; 227 | ticm20689_pkg->ay = RotMat[1][0]*facc_t[0] + RotMat[1][1]*facc_t[1] + RotMat[1][2]*facc_t[2]; 228 | ticm20689_pkg->az = RotMat[2][0]*facc_t[0] + RotMat[2][1]*facc_t[1] + RotMat[2][2]*facc_t[2]; 229 | 230 | 231 | ticm20689_pkg->timestamp = timestamp - IMU_DELAY_OFFSET; 232 | 233 | } 234 | 235 | 236 | 237 | static int ce_imu_cmd_icm20689_start() 238 | { 239 | 240 | unsigned char tcmd[10]; 241 | 242 | tcmd[0] = CMD_HEAD1; 243 | tcmd[1] = CMD_HEAD2; 244 | tcmd[2] = CMD_IMC20689_START; 245 | 246 | short int acc_offset[3] ; 247 | 248 | acc_offset[0] = (short int)ce_config_get_cf_imu_icm20689_acc_bias_X(); 249 | acc_offset[1] = (short int)ce_config_get_cf_imu_icm20689_acc_bias_Y(); 250 | acc_offset[2] = (short int)ce_config_get_cf_imu_icm20689_acc_bias_Z(); 251 | 252 | memcpy(&tcmd[3], acc_offset, 6); 253 | 254 | tcmd[9] = 0; 255 | for(int i=3; i<9; i++) 256 | { 257 | tcmd[9] += tcmd[i]; 258 | } 259 | 260 | int ret = write(ce_imu_uart_fd, tcmd, 10); 261 | usleep(10000); 262 | return ret; 263 | } 264 | 265 | 266 | 267 | static int ce_imu_cmd_sample_rate() 268 | { 269 | unsigned char tcmd[10] = {0,0,0,0,0, 0,0,0,0,0}; 270 | 271 | tcmd[0] = CMD_HEAD1; 272 | tcmd[1] = CMD_HEAD2; 273 | tcmd[2] = CMD_IMC20689_SAMPLE_RATE; 274 | 275 | tcmd[3] = 1000/ce_config_get_cf_imu_icm20689_sample_rate() - 1 ; 276 | 277 | tcmd[9] = 0; 278 | for(int i=3; i<9; i++) 279 | { 280 | tcmd[9] += tcmd[i]; 281 | } 282 | 283 | int ret = write(ce_imu_uart_fd, tcmd, 10); 284 | usleep(10000); 285 | return ret; 286 | } 287 | 288 | 289 | 290 | void *ce_imu_data_feed(void*) 291 | { 292 | unsigned char imu_frame[IMU_FRAME_LEN]; 293 | unsigned char imu_frame_buf[2*IMU_FRAME_LEN]; 294 | 295 | memset(imu_frame, 0, IMU_FRAME_LEN); 296 | memset(imu_frame_buf, 0, 2*IMU_FRAME_LEN); 297 | 298 | double time_interval = (double)1/ce_config_get_cf_imu_icm20689_sample_rate(); 299 | double timestamp; 300 | double start_time; 301 | double last_time; 302 | double new_time; 303 | double last_pred; 304 | double new_pred; 305 | 306 | struct timeval getIMUTime; 307 | 308 | //tcflush(ce_imu_uart_fd, TCIFLUSH); 309 | gettimeofday(&getIMUTime, NULL); 310 | 311 | 312 | start_time = getIMUTime.tv_sec + 0.000001 * getIMUTime.tv_usec; 313 | 314 | new_time = start_time; 315 | new_pred = start_time; 316 | 317 | last_time = start_time - (double)1/ce_config_get_cf_imu_icm20689_sample_rate(); 318 | last_pred = start_time - (double)1/ce_config_get_cf_imu_icm20689_sample_rate(); 319 | 320 | int num_get = 0; 321 | 322 | tcflush(ce_imu_uart_fd, TCIFLUSH); 323 | while(!ce_imu_capture_stop_run) 324 | { 325 | start_time = getIMUTime.tv_sec + 0.000001 * getIMUTime.tv_usec; 326 | 327 | while(num_get < IMU_FRAME_LEN) 328 | { 329 | int temp_read_get = read(ce_imu_uart_fd, &imu_frame_buf[num_get], IMU_FRAME_LEN); 330 | if (0 >= temp_read_get) 331 | { 332 | break; 333 | } 334 | num_get += temp_read_get; 335 | } 336 | 337 | if (0 == num_get) 338 | continue; 339 | 340 | //std::cout << "start_time :"<< std::setprecision(15) << start_time < IMU_FRAME_LEN) 387 | { 388 | tcflush(ce_imu_uart_fd, TCIFLUSH); 389 | memset(imu_frame_buf, 0, 2*IMU_FRAME_LEN); 390 | num_get = 0; 391 | continue; 392 | } 393 | 394 | new_time = getIMUTime.tv_sec + 0.000001*getIMUTime.tv_usec; 395 | 396 | double newtime_interval = new_time-last_time; 397 | if(newtime_interval<0.004 || newtime_interval>0.006) 398 | newtime_interval = 0.005; 399 | time_interval = time_interval*0.999 + 0.001*newtime_interval; 400 | 401 | if(new_time-last_pred < time_interval) 402 | new_pred = new_time; 403 | else 404 | new_pred = last_pred + time_interval + 0.001*(new_time - last_pred - time_interval); 405 | 406 | last_time = new_time; 407 | last_pred = new_pred; 408 | 409 | memcpy(imu_frame, &imu_frame_buf[position_pkghead], IMU_FRAME_LEN); 410 | timestamp = new_pred; 411 | 412 | icm20689_pkg *ticm20689_pkg = new icm20689_pkg; 413 | if (NULL == ticm20689_pkg) 414 | { 415 | printf("ce_imu_data_feed alloc memory failure! exit thread!\n"); 416 | break; 417 | } 418 | memset(ticm20689_pkg, 0, sizeof(ticm20689_pkg)); 419 | 420 | ce_imu_format_imu_frame(imu_frame, timestamp, ticm20689_pkg); 421 | 422 | 423 | icm20689_pkg *icm20689_giveup = NULL; 424 | icm20689_pkg_list.push(ticm20689_pkg, icm20689_giveup, IMU_FIFO_SIZE - 5); 425 | if (NULL != icm20689_giveup) 426 | { 427 | delete icm20689_giveup; 428 | icm20689_giveup = NULL; 429 | } 430 | 431 | memset(imu_frame_buf, 0, 2*IMU_FRAME_LEN); 432 | num_get = 0; 433 | 434 | } 435 | 436 | ce_imu_capture_thread = 0; 437 | pthread_exit(NULL); 438 | } 439 | 440 | int ce_imu_capture_init() 441 | { 442 | int fd = ce_imu_open_uart(ce_config_get_cf_imu_uart().c_str()); 443 | if(fd < 0) 444 | { 445 | printf("celog: open_uart failed !\r\n"); 446 | return ERROR; 447 | } 448 | 449 | if(ce_imu_set_uart(fd, 460800, 8, 'N', 1) < 0) 450 | { 451 | printf("celog: set_uart failed !\r\n"); 452 | return ERROR; 453 | } 454 | 455 | ce_imu_uart_fd = fd; 456 | 457 | if(ERROR == ce_imu_cmd_icm20689_start()) 458 | { 459 | printf("celog: icm20689_start failed !\r\n"); 460 | return ERROR; 461 | } 462 | 463 | 464 | 465 | if(ERROR == ce_imu_cmd_sample_rate()) 466 | { 467 | printf("celog: icm20689 set sample rate failed !\r\n"); 468 | return ERROR; 469 | } 470 | 471 | 472 | int temp; 473 | temp = pthread_create(&ce_imu_capture_thread, NULL, ce_imu_data_feed, NULL); 474 | if( temp ) 475 | { 476 | printf("celog: Thread imu creat fail !\r\n"); 477 | return ERROR; 478 | } 479 | 480 | return SUCCESS; 481 | } 482 | 483 | void ce_imu_capture_close() 484 | { 485 | ce_imu_capture_stop_run = true; 486 | usleep(10000); 487 | 488 | if(ce_imu_capture_thread !=0) 489 | { 490 | pthread_join(ce_imu_capture_thread, NULL); 491 | } 492 | } 493 | 494 | static void* ce_imu_showdata(void *) 495 | { 496 | icm20689_pkg *ticm20689_pkg; 497 | 498 | CCpuSet::instance()->SetCpu(0, "imu_show"); 499 | 500 | while(!ce_imu_showdata_stop_run) 501 | { 502 | if(!icm20689_pkg_list.try_pop(ticm20689_pkg)) 503 | { 504 | usleep(100); 505 | continue; 506 | } 507 | 508 | std::cout << "imu_stamp :" << std::setprecision(15)<< ticm20689_pkg->timestamp << std::endl; 509 | // std::cout << "Acc X :" << ticm20689_pkg->ax << std::endl; 510 | // std::cout << "Acc Y :" << ticm20689_pkg->ay << std::endl; 511 | // std::cout << "Acc Z :" << ticm20689_pkg->az << std::endl; 512 | // 513 | // std::cout << "Gyr X :" << ticm20689_pkg->rx << std::endl; 514 | // std::cout << "Gyr Y :" << ticm20689_pkg->ry << std::endl; 515 | // std::cout << "Gyr Z :" << ticm20689_pkg->rz << std::endl; 516 | 517 | // MadgwickAHRSupdateIMU( 3.1415926f * ticm20689_pkg->rx / 180.0f, 518 | // 3.1415926f * ticm20689_pkg->ry / 180.0f, 519 | // 3.1415926f * ticm20689_pkg->rz / 180.0f, 520 | // ticm20689_pkg->ax, 521 | // ticm20689_pkg->ay, 522 | // ticm20689_pkg->az); 523 | 524 | delete ticm20689_pkg; 525 | ticm20689_pkg = NULL; 526 | 527 | 528 | } 529 | ce_imu_showdata_thread = 0; 530 | pthread_exit(NULL); 531 | } 532 | 533 | int ce_imu_showdata_init() 534 | { 535 | int temp = pthread_create(&ce_imu_showdata_thread, NULL, ce_imu_showdata, NULL); 536 | if(temp) 537 | { 538 | printf("Failed to create thread show_imuData\r\n"); 539 | return ERROR; 540 | } 541 | return SUCCESS; 542 | } 543 | 544 | void ce_imu_showdata_close() 545 | { 546 | ce_imu_showdata_stop_run = true; 547 | usleep(100); 548 | 549 | if(ce_imu_showdata_thread != 0) 550 | { 551 | pthread_join(ce_imu_showdata_thread,NULL); 552 | } 553 | } 554 | 555 | 556 | void ce_imu_clear_list() 557 | { 558 | 559 | icm20689_pkg *ticm20689_pkg; 560 | int tsize = 0; 561 | 562 | while(icm20689_pkg_list.size() != 0) 563 | { 564 | if(!icm20689_pkg_list.try_pop(ticm20689_pkg)) 565 | { 566 | continue; 567 | } 568 | 569 | delete ticm20689_pkg; 570 | ticm20689_pkg = NULL; 571 | tsize = icm20689_pkg_list.size(); 572 | 573 | } 574 | 575 | } 576 | 577 | 578 | void ce_imu_get_acc_data(short int *ptr) 579 | { 580 | int cnt = 0; 581 | short int *ptr_raw; 582 | ptr_raw = ptr; 583 | icm20689_pkg *ticm20689_pkg; 584 | 585 | ce_imu_clear_list(); 586 | 587 | while(cnt<=1024) 588 | { 589 | 590 | if(!icm20689_pkg_list.try_pop(ticm20689_pkg)) 591 | { 592 | usleep(10); 593 | continue; 594 | } 595 | 596 | 597 | float norm_gyro = sqrt( ticm20689_pkg->rx * ticm20689_pkg->rx + 598 | ticm20689_pkg->ry * ticm20689_pkg->ry + 599 | ticm20689_pkg->rz * ticm20689_pkg->rz); 600 | 601 | if(norm_gyro > 10) 602 | { 603 | std::cout << "celog: don't move the camera..." << std::endl; 604 | cnt = 0; 605 | ptr = ptr_raw; 606 | sleep(1); 607 | continue; 608 | } 609 | 610 | 611 | 612 | *ptr = (short int)(ticm20689_pkg->ax/STD_g*8192); 613 | ptr++; 614 | *ptr = (short int)(ticm20689_pkg->ay/STD_g*8192); 615 | ptr++; 616 | *ptr = (short int)(ticm20689_pkg->az/STD_g*8192); 617 | ptr++; 618 | 619 | delete ticm20689_pkg; 620 | ticm20689_pkg = NULL; 621 | cnt++; 622 | 623 | if(cnt%128 == 0) 624 | { 625 | std::cout << "cnt :" << cnt <<"/1024" << std::endl; 626 | } 627 | 628 | } 629 | } 630 | 631 | 632 | 633 | void ce_imu_get_gyro_data(short int *ptr) 634 | { 635 | int cnt = 0; 636 | short int *ptr_raw; 637 | ptr_raw = ptr; 638 | 639 | icm20689_pkg *ticm20689_pkg; 640 | 641 | 642 | ce_imu_clear_list(); 643 | 644 | while(cnt<=1024*5) 645 | { 646 | 647 | if(!icm20689_pkg_list.try_pop(ticm20689_pkg)) 648 | { 649 | usleep(10); 650 | continue; 651 | } 652 | 653 | 654 | float norm_gyro = sqrt( ticm20689_pkg->rx * ticm20689_pkg->rx + 655 | ticm20689_pkg->ry * ticm20689_pkg->ry + 656 | ticm20689_pkg->rz * ticm20689_pkg->rz); 657 | 658 | if(norm_gyro > 10) 659 | { 660 | std::cout << "celog: don't move the camera..." << std::endl; 661 | cnt = 0; 662 | ptr = ptr_raw; 663 | sleep(1); 664 | continue; 665 | } 666 | 667 | *ptr = (short int)(ticm20689_pkg->rx/500*32768); 668 | ptr++; 669 | *ptr = (short int)(ticm20689_pkg->ry/500*32768); 670 | ptr++; 671 | *ptr = (short int)(ticm20689_pkg->rz/500*32768); 672 | ptr++; 673 | 674 | delete ticm20689_pkg; 675 | ticm20689_pkg = NULL; 676 | cnt++; 677 | 678 | if(cnt%512 == 0) 679 | { 680 | std::cout << "cnt :" << cnt <<"/5120" << std::endl; 681 | } 682 | 683 | 684 | } 685 | } 686 | -------------------------------------------------------------------------------- /sdk/src/cedriver_usb.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "cedriver_usb.h" 5 | 6 | static int ce_usb_camd1_num; 7 | static struct ce_usb_dev_stru ce_usb_dev[MAXDEVICES]; 8 | static libusb_device **libusb_device_list; 9 | 10 | static int ce_usb_lookup_cam_handle(libusb_device *dev) 11 | { 12 | struct libusb_device_descriptor desc; 13 | libusb_get_device_descriptor(dev, &desc); 14 | if ( (VID_CE_D1 == desc.idVendor) && (PID_CE_D1 == desc.idProduct) ) 15 | return SUCCESS; 16 | return ERROR; 17 | } 18 | 19 | libusb_device_handle * ce_usb_gethandle(int index) 20 | { 21 | return ce_usb_dev[index].handle; 22 | } 23 | 24 | int ce_usb_open(void) 25 | { 26 | int r; 27 | 28 | r = libusb_init(NULL); 29 | if (r) 30 | { 31 | printf("celog: init libusb Error !\n"); 32 | return -1; 33 | } 34 | 35 | int cnt_dev = libusb_get_device_list(NULL, &libusb_device_list); 36 | if ( cnt_dev < 0 ) 37 | { 38 | printf("celog: Error in enumerating devices !\n"); 39 | return -2; 40 | } 41 | 42 | ce_usb_camd1_num = 0; 43 | 44 | for (int i = 0; i < cnt_dev; ++i ) 45 | { 46 | libusb_device *tdev = libusb_device_list[i]; 47 | if ( SUCCESS == ce_usb_lookup_cam_handle(tdev) ) 48 | { 49 | ce_usb_dev[ce_usb_camd1_num].dev = tdev; 50 | r = libusb_open(tdev, &ce_usb_dev[ce_usb_camd1_num].handle); 51 | if ( r ) 52 | { 53 | printf("celog: open usb camD1 fail , r=%d\n",r); 54 | return -3; 55 | } 56 | printf("+++++++ce_usb_open i=%d, point=%p!\n", ce_usb_camd1_num, ce_usb_dev[ce_usb_camd1_num].handle); 57 | ++ce_usb_camd1_num; 58 | } 59 | if(ce_usb_camd1_num>=MAXDEVICES) 60 | break; 61 | } 62 | return ce_usb_camd1_num; 63 | } 64 | 65 | void ce_usb_close(void) 66 | { 67 | for (int i = 0; i < ce_usb_camd1_num; ++i ) 68 | { 69 | if (ce_usb_dev[i].handle) 70 | { 71 | libusb_close(ce_usb_dev[i].handle); 72 | ce_usb_dev[i].handle = NULL; 73 | } 74 | 75 | printf("ce_usb_close ,idx=%d, point=%p!\n", i, ce_usb_dev[i].handle); 76 | memset(&ce_usb_dev[i], 0, sizeof(ce_usb_dev[i])); 77 | } 78 | libusb_free_device_list(libusb_device_list, 1); 79 | libusb_exit(NULL); 80 | } 81 | 82 | void ce_usb_close(int i) 83 | { 84 | if (i >= ce_usb_camd1_num) 85 | return; 86 | 87 | if (ce_usb_dev[i].handle) 88 | { 89 | libusb_close(ce_usb_dev[i].handle); 90 | ce_usb_dev[i].handle = NULL; 91 | } 92 | 93 | printf("ce_usb_close ,idx=%d, point=%p!\n", i, ce_usb_dev[i].handle); 94 | memset(&ce_usb_dev[i], 0, sizeof(ce_usb_dev[i])); 95 | } 96 | 97 | -------------------------------------------------------------------------------- /sdk/src/celib_MadgwickAHRS.cpp: -------------------------------------------------------------------------------- 1 | //===================================================================================================== 2 | // MadgwickAHRS.c 3 | //===================================================================================================== 4 | // 5 | // Implementation of Madgwick's IMU and AHRS algorithms. 6 | // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 7 | // 8 | // Date Author Notes 9 | // 29/09/2011 SOH Madgwick Initial release 10 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 11 | // 19/02/2012 SOH Madgwick Magnetometer measurement is normalised 12 | // 13 | //===================================================================================================== 14 | 15 | //--------------------------------------------------------------------------------------------------- 16 | // Header files 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | #include "celib_MadgwickAHRS.h" 28 | #include 29 | #include 30 | #include 31 | 32 | //--------------------------------------------------------------------------------------------------- 33 | // Definitions 34 | 35 | #define sampleFreq 1000.0f // sample frequency in Hz 36 | #define betaDef 0.3f // 2 * proportional gain 37 | 38 | //--------------------------------------------------------------------------------------------------- 39 | // Variable definitions 40 | 41 | volatile float beta = betaDef; // 2 * proportional gain (Kp) 42 | volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame 43 | 44 | //--------------------------------------------------------------------------------------------------- 45 | // Function declarations 46 | 47 | float invSqrt(float x); 48 | 49 | //==================================================================================================== 50 | // Functions 51 | 52 | //--------------------------------------------------------------------------------------------------- 53 | // AHRS algorithm update 54 | 55 | void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { 56 | float recipNorm; 57 | float s0, s1, s2, s3; 58 | float qDot1, qDot2, qDot3, qDot4; 59 | float hx, hy; 60 | float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; 61 | 62 | // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) 63 | if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { 64 | MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az); 65 | return; 66 | } 67 | 68 | // Rate of change of quaternion from gyroscope 69 | qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); 70 | qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); 71 | qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); 72 | qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); 73 | 74 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 75 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 76 | 77 | // Normalise accelerometer measurement 78 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); 79 | ax *= recipNorm; 80 | ay *= recipNorm; 81 | az *= recipNorm; 82 | 83 | // Normalise magnetometer measurement 84 | recipNorm = invSqrt(mx * mx + my * my + mz * mz); 85 | mx *= recipNorm; 86 | my *= recipNorm; 87 | mz *= recipNorm; 88 | 89 | // Auxiliary variables to avoid repeated arithmetic 90 | _2q0mx = 2.0f * q0 * mx; 91 | _2q0my = 2.0f * q0 * my; 92 | _2q0mz = 2.0f * q0 * mz; 93 | _2q1mx = 2.0f * q1 * mx; 94 | _2q0 = 2.0f * q0; 95 | _2q1 = 2.0f * q1; 96 | _2q2 = 2.0f * q2; 97 | _2q3 = 2.0f * q3; 98 | _2q0q2 = 2.0f * q0 * q2; 99 | _2q2q3 = 2.0f * q2 * q3; 100 | q0q0 = q0 * q0; 101 | q0q1 = q0 * q1; 102 | q0q2 = q0 * q2; 103 | q0q3 = q0 * q3; 104 | q1q1 = q1 * q1; 105 | q1q2 = q1 * q2; 106 | q1q3 = q1 * q3; 107 | q2q2 = q2 * q2; 108 | q2q3 = q2 * q3; 109 | q3q3 = q3 * q3; 110 | 111 | // Reference direction of Earth's magnetic field 112 | hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; 113 | hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; 114 | _2bx = sqrt(hx * hx + hy * hy); 115 | _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; 116 | _4bx = 2.0f * _2bx; 117 | _4bz = 2.0f * _2bz; 118 | 119 | // Gradient decent algorithm corrective step 120 | s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 121 | s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 122 | s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 123 | s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 124 | recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 125 | s0 *= recipNorm; 126 | s1 *= recipNorm; 127 | s2 *= recipNorm; 128 | s3 *= recipNorm; 129 | 130 | // Apply feedback step 131 | qDot1 -= beta * s0; 132 | qDot2 -= beta * s1; 133 | qDot3 -= beta * s2; 134 | qDot4 -= beta * s3; 135 | } 136 | 137 | // Integrate rate of change of quaternion to yield quaternion 138 | q0 += qDot1 * (1.0f / sampleFreq); 139 | q1 += qDot2 * (1.0f / sampleFreq); 140 | q2 += qDot3 * (1.0f / sampleFreq); 141 | q3 += qDot4 * (1.0f / sampleFreq); 142 | 143 | // Normalise quaternion 144 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 145 | q0 *= recipNorm; 146 | q1 *= recipNorm; 147 | q2 *= recipNorm; 148 | q3 *= recipNorm; 149 | } 150 | 151 | //--------------------------------------------------------------------------------------------------- 152 | // IMU algorithm update 153 | 154 | void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { 155 | float recipNorm; 156 | float s0, s1, s2, s3; 157 | float qDot1, qDot2, qDot3, qDot4; 158 | float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; 159 | 160 | // Rate of change of quaternion from gyroscope 161 | qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); 162 | qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); 163 | qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); 164 | qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); 165 | 166 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 167 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 168 | 169 | // Normalise accelerometer measurement 170 | //recipNorm = invSqrt(ax * ax + ay * ay + az * az); 171 | recipNorm = 1/sqrt(ax * ax + ay * ay + az * az); 172 | ax *= recipNorm; 173 | ay *= recipNorm; 174 | az *= recipNorm; 175 | 176 | 177 | // Auxiliary variables to avoid repeated arithmetic 178 | _2q0 = 2.0f * q0; 179 | _2q1 = 2.0f * q1; 180 | _2q2 = 2.0f * q2; 181 | _2q3 = 2.0f * q3; 182 | _4q0 = 4.0f * q0; 183 | _4q1 = 4.0f * q1; 184 | _4q2 = 4.0f * q2; 185 | _8q1 = 8.0f * q1; 186 | _8q2 = 8.0f * q2; 187 | q0q0 = q0 * q0; 188 | q1q1 = q1 * q1; 189 | q2q2 = q2 * q2; 190 | q3q3 = q3 * q3; 191 | 192 | // Gradient decent algorithm corrective step 193 | s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; 194 | s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; 195 | s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; 196 | s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; 197 | //recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 198 | recipNorm = 1/sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 199 | s0 *= recipNorm; 200 | s1 *= recipNorm; 201 | s2 *= recipNorm; 202 | s3 *= recipNorm; 203 | 204 | // Apply feedback step 205 | qDot1 -= beta * s0; 206 | qDot2 -= beta * s1; 207 | qDot3 -= beta * s2; 208 | qDot4 -= beta * s3; 209 | } 210 | 211 | // Integrate rate of change of quaternion to yield quaternion 212 | q0 += qDot1 * (1.0f / ce_config_get_cf_imu_icm20689_sample_rate()); 213 | q1 += qDot2 * (1.0f / ce_config_get_cf_imu_icm20689_sample_rate()); 214 | q2 += qDot3 * (1.0f / ce_config_get_cf_imu_icm20689_sample_rate()); 215 | q3 += qDot4 * (1.0f / ce_config_get_cf_imu_icm20689_sample_rate()); 216 | 217 | // Normalise quaternion 218 | //recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 219 | recipNorm = 1/sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 220 | q0 *= recipNorm; 221 | q1 *= recipNorm; 222 | q2 *= recipNorm; 223 | q3 *= recipNorm; 224 | 225 | 226 | float Pitch = (float)-asin(-2*q1*q3 + 2*q0*q2) * 57.3; 227 | float Roll = (float)atan2( 2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1) * 57.3; 228 | float Yaw = (float)atan2(2*q1*q2 + 2*q0*q3, q0*q0 + q1*q1 - q2*q2 - q3*q3) * 57.3; 229 | 230 | std::cout << "Pitch =" << Pitch < src2.rows ? src1.rows : src2.rows; 28 | int cols = src1.cols + 20 + src2.cols; 29 | 30 | cv::Mat zeroMat = cv::Mat::zeros(rows, cols, src1.type()); 31 | zeroMat.copyTo(dst); 32 | src1.copyTo(dst(cv::Rect(0,0,src1.cols,src1.rows))); 33 | src2.copyTo(dst(cv::Rect(src1.cols+20,0,src2.cols,src2.rows))); 34 | 35 | // double score=89.101; 36 | // char info[256]; 37 | // sprintf(info,"score=%.2f",score); 38 | // cv::putText(dst,info,cv::Point(2,50),CV_FONT_HERSHEY_COMPLEX,1,cv::Scalar(255,0,0)); 39 | } 40 | 41 | 42 | void ce_depth2color(cv::Mat & color, const cv::Mat & depth, const double max, const double min) 43 | { 44 | cv::Mat grayImage; 45 | double alpha = 255.0 / (max - min); 46 | depth.convertTo(grayImage, CV_8UC1, alpha, -alpha * min ); 47 | cv::applyColorMap(grayImage, color, cv::COLORMAP_JET); 48 | } 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /sdk/src/ceros_cam_d1_driver.cpp: -------------------------------------------------------------------------------- 1 | #include "ceros_cam_d1_driver.h" 2 | 3 | #include "cedriver_usb.h" 4 | #include "cedriver_cam.h" 5 | #include "cedriver_imu.h" 6 | #include "cedriver_config.h" 7 | #include "threadsafe_queue.h" 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | extern threadsafe_queue img_pkg_list_d1; 14 | extern threadsafe_queue icm20689_pkg_list; 15 | 16 | extern threadsafe_queue img_pkg_list_s1num0; 17 | extern threadsafe_queue img_pkg_list_s1num1; 18 | extern threadsafe_queue img_pkg_list_s1num2; 19 | 20 | 21 | CoolEyeCamD1::CoolEyeCamD1(ros::NodeHandle nh, char path[]):cam_thread_stop_run_(false),imu_thread_stop_run_(false) 22 | { 23 | 24 | ce_config_load_settings(path); 25 | 26 | pub_imu_ = nh.advertise("imu0_icm20689",ce_config_get_cf_imu_icm20689_sample_rate()); 27 | 28 | image_transport::ImageTransport imageTransL(nh); 29 | pub_caml_ = imageTransL.advertise("/cooleyed1/left/image_raw", 1); 30 | 31 | image_transport::ImageTransport imageTransR(nh); 32 | pub_camr_ = imageTransR.advertise("/cooleyed1/right/image_raw", 1); 33 | 34 | 35 | image_transport::ImageTransport imageTransS1N0(nh); 36 | pub_cams1n0_ = imageTransS1N0.advertise("/cooleyes1/num0/image_raw", 1); 37 | 38 | image_transport::ImageTransport imageTransS1N1(nh); 39 | pub_cams1n1_ = imageTransS1N0.advertise("/cooleyes1/num1/image_raw", 1); 40 | 41 | image_transport::ImageTransport imageTransS1N2(nh); 42 | pub_cams1n2_ = imageTransS1N0.advertise("/cooleyes1/num2/image_raw", 1); 43 | 44 | 45 | 46 | 47 | img_left_.create(cv::Size(ce_config_get_cf_img_width(),ce_config_get_cf_img_height()),CV_8UC1); 48 | img_right_.create(cv::Size(ce_config_get_cf_img_width(),ce_config_get_cf_img_height()),CV_8UC1); 49 | left_bridge_ = cv_bridge::CvImage(std_msgs::Header(), "mono8", img_left_); 50 | right_bridge_ = cv_bridge::CvImage(std_msgs::Header(), "mono8", img_right_); 51 | 52 | 53 | 54 | 55 | img_s1n0_.create(cv::Size(ce_config_get_cf_img_width(),ce_config_get_cf_img_height()),CV_8UC1); 56 | s1n0_bridge_ = cv_bridge::CvImage(std_msgs::Header(), "mono8", img_s1n0_); 57 | 58 | img_s1n1_.create(cv::Size(ce_config_get_cf_img_width(),ce_config_get_cf_img_height()),CV_8UC1); 59 | s1n1_bridge_ = cv_bridge::CvImage(std_msgs::Header(), "mono8", img_s1n1_); 60 | 61 | img_s1n2_.create(cv::Size(ce_config_get_cf_img_width(),ce_config_get_cf_img_height()),CV_8UC1); 62 | s1n2_bridge_ = cv_bridge::CvImage(std_msgs::Header(), "mono8", img_s1n2_); 63 | 64 | 65 | } 66 | 67 | bool CoolEyeCamD1::start_cam_D1_capture() 68 | { 69 | return ce_cam_capture_init() < 0 ? false : true; 70 | } 71 | 72 | 73 | bool CoolEyeCamD1::start_cam_D1_preprocess() 74 | { 75 | return ce_cam_preprocess_init() < 0 ? false : true; 76 | } 77 | 78 | 79 | 80 | bool CoolEyeCamD1::start_imu_icm26089() 81 | { 82 | return ce_imu_capture_init() < 0 ? false : true; 83 | } 84 | 85 | 86 | 87 | void CoolEyeCamD1::img_data_stream ( ) 88 | { 89 | ros::NodeHandle cam_n; 90 | 91 | ros::Rate loop_rate(1000); 92 | 93 | sensor_msgs::ImagePtr msg_l, msg_r, msg_s1n0, msg_s1n1, msg_s1n2; 94 | ros::Time msg_time; 95 | double left_timestamp, right_timestamp, s1n0_timestamp, s1n1_timestamp, s1n2_timestamp; 96 | 97 | d1_img_output_pkg *img_lr_pkg; 98 | 99 | s1_img_output_pkg *img_s1n0_pkg; 100 | s1_img_output_pkg *img_s1n1_pkg; 101 | s1_img_output_pkg *img_s1n2_pkg; 102 | 103 | 104 | while(cam_n.ok()) 105 | { 106 | //////////////////////////////////////////// 107 | 108 | // new D1 109 | if(img_pkg_list_d1.try_pop(img_lr_pkg)) 110 | { 111 | memcpy(img_left_.data, img_lr_pkg->left_img->data, ce_config_get_cf_img_size()); 112 | left_timestamp = img_lr_pkg->left_img->timestamp; 113 | 114 | double tmpl = floor(left_timestamp); 115 | msg_time.sec=(__time_t) tmpl; 116 | msg_time.nsec=(__time_t)((left_timestamp - tmpl)* 1e+9); 117 | 118 | left_bridge_.header.stamp = msg_time; 119 | left_bridge_.header.seq=0; 120 | 121 | memcpy(img_right_.data, img_lr_pkg->right_img->data, ce_config_get_cf_img_size()); 122 | right_timestamp = img_lr_pkg->right_img->timestamp; 123 | 124 | double tmpr = floor(right_timestamp); 125 | msg_time.sec=(__time_t) tmpr; 126 | msg_time.nsec=(__time_t)((right_timestamp - tmpr)* 1e+9); 127 | 128 | right_bridge_.header.stamp = msg_time; 129 | right_bridge_.header.seq=0; 130 | 131 | msg_l = left_bridge_.toImageMsg(); 132 | msg_r = right_bridge_.toImageMsg(); 133 | 134 | pub_caml_.publish(msg_l); 135 | pub_camr_.publish(msg_r); 136 | 137 | 138 | if(ce_config_get_cf_ros_showimage()) 139 | { 140 | cv::imshow("left",img_left_); 141 | cv::imshow("right",img_right_); 142 | cv::waitKey(1); 143 | } 144 | 145 | delete img_lr_pkg->left_img; 146 | delete img_lr_pkg->right_img; 147 | delete img_lr_pkg; 148 | img_lr_pkg = NULL; 149 | } 150 | 151 | // new S1N0 152 | if(img_pkg_list_s1num0.try_pop(img_s1n0_pkg)) 153 | { 154 | memcpy(img_s1n0_.data, img_s1n0_pkg->s1_img->data, ce_config_get_cf_img_size()); 155 | s1n0_timestamp = img_s1n0_pkg->s1_img->timestamp; 156 | 157 | double tmps1n0 = floor(s1n0_timestamp); 158 | msg_time.sec=(__time_t) tmps1n0; 159 | msg_time.nsec=(__time_t)((s1n0_timestamp - tmps1n0)* 1e+9); 160 | 161 | s1n0_bridge_.header.stamp = msg_time; 162 | s1n0_bridge_.header.seq = 0; 163 | 164 | 165 | msg_s1n0 = s1n0_bridge_.toImageMsg(); 166 | 167 | pub_cams1n0_.publish(msg_s1n0); 168 | 169 | 170 | if(ce_config_get_cf_ros_showimage()) 171 | { 172 | cv::imshow("s1n0",img_s1n0_); 173 | cv::waitKey(1); 174 | } 175 | 176 | delete img_s1n0_pkg->s1_img; 177 | delete img_s1n0_pkg; 178 | img_s1n0_pkg = NULL; 179 | } 180 | 181 | 182 | // new S1N1 183 | if(img_pkg_list_s1num1.try_pop(img_s1n1_pkg)) 184 | { 185 | memcpy(img_s1n1_.data, img_s1n1_pkg->s1_img->data, ce_config_get_cf_img_size()); 186 | s1n1_timestamp = img_s1n1_pkg->s1_img->timestamp; 187 | 188 | double tmps1n1 = floor(s1n1_timestamp); 189 | msg_time.sec=(__time_t) tmps1n1; 190 | msg_time.nsec=(__time_t)((s1n1_timestamp - tmps1n1)* 1e+9); 191 | 192 | s1n1_bridge_.header.stamp = msg_time; 193 | s1n1_bridge_.header.seq = 0; 194 | 195 | 196 | msg_s1n1 = s1n1_bridge_.toImageMsg(); 197 | 198 | pub_cams1n1_.publish(msg_s1n1); 199 | 200 | 201 | if(ce_config_get_cf_ros_showimage()) 202 | { 203 | cv::imshow("s1n1",img_s1n1_); 204 | cv::waitKey(1); 205 | } 206 | 207 | delete img_s1n1_pkg->s1_img; 208 | delete img_s1n1_pkg; 209 | img_s1n1_pkg = NULL; 210 | } 211 | 212 | 213 | // new S1N2 214 | if(img_pkg_list_s1num2.try_pop(img_s1n2_pkg)) 215 | { 216 | memcpy(img_s1n2_.data, img_s1n2_pkg->s1_img->data, ce_config_get_cf_img_size()); 217 | s1n2_timestamp = img_s1n2_pkg->s1_img->timestamp; 218 | 219 | double tmps1n2 = floor(s1n2_timestamp); 220 | msg_time.sec=(__time_t) tmps1n2; 221 | msg_time.nsec=(__time_t)((s1n2_timestamp - tmps1n2)* 1e+9); 222 | 223 | s1n2_bridge_.header.stamp = msg_time; 224 | s1n2_bridge_.header.seq = 0; 225 | 226 | 227 | msg_s1n2 = s1n2_bridge_.toImageMsg(); 228 | 229 | pub_cams1n2_.publish(msg_s1n2); 230 | 231 | 232 | if(ce_config_get_cf_ros_showimage()) 233 | { 234 | cv::imshow("s1n2",img_s1n2_); 235 | cv::waitKey(1); 236 | } 237 | 238 | delete img_s1n2_pkg->s1_img; 239 | delete img_s1n2_pkg; 240 | img_s1n2_pkg = NULL; 241 | } 242 | 243 | loop_rate.sleep(); 244 | 245 | //ros::spinOnce(); 246 | } 247 | } 248 | 249 | void CoolEyeCamD1:: imu_data_stream( ) 250 | { 251 | ros::NodeHandle imu_n; 252 | sensor_msgs::Imu imu_msg; 253 | ros::Time imu_time; 254 | 255 | ros::Rate loop_rate(2000); 256 | 257 | icm20689_pkg *ticm20689_pkg; 258 | while(imu_n.ok()) 259 | { 260 | if(!icm20689_pkg_list.try_pop(ticm20689_pkg)) 261 | { 262 | loop_rate.sleep(); 263 | continue; 264 | } 265 | 266 | 267 | double tmp = floor(ticm20689_pkg->timestamp); 268 | imu_time.sec=(__time_t) tmp; 269 | imu_time.nsec=(__time_t)((ticm20689_pkg->timestamp - tmp)* 1e+9); 270 | 271 | imu_msg.header.frame_id = "/imu"; 272 | imu_msg.header.stamp = imu_time; 273 | imu_msg.header.seq = 0; 274 | imu_msg.linear_acceleration.x = ticm20689_pkg->ax; 275 | imu_msg.linear_acceleration.y = ticm20689_pkg->ay; 276 | imu_msg.linear_acceleration.z = ticm20689_pkg->az; 277 | 278 | //imu_msg.linear_acceleration_covariance[0] = 0.04; 279 | imu_msg.linear_acceleration_covariance[0] = 0; 280 | imu_msg.linear_acceleration_covariance[1] = 0; 281 | imu_msg.linear_acceleration_covariance[2] = 0; 282 | 283 | imu_msg.linear_acceleration_covariance[3] = 0; 284 | //imu_msg.linear_acceleration_covariance[4] = 0.04; 285 | imu_msg.linear_acceleration_covariance[4] = 0; 286 | imu_msg.linear_acceleration_covariance[5] = 0; 287 | 288 | imu_msg.linear_acceleration_covariance[6] = 0; 289 | imu_msg.linear_acceleration_covariance[7] = 0; 290 | //imu_msg.linear_acceleration_covariance[8] = 0.04; 291 | imu_msg.linear_acceleration_covariance[8] = 0; 292 | 293 | imu_msg.angular_velocity.x = 3.1415926f * ticm20689_pkg->rx / 180.0f; 294 | imu_msg.angular_velocity.y = 3.1415926f * ticm20689_pkg->ry / 180.0f; 295 | imu_msg.angular_velocity.z = 3.1415926f * ticm20689_pkg->rz / 180.0f; 296 | 297 | //imu_msg.angular_velocity_covariance[0] = 0.02; 298 | imu_msg.angular_velocity_covariance[0] = 0; 299 | imu_msg.angular_velocity_covariance[1] = 0; 300 | imu_msg.angular_velocity_covariance[2] = 0; 301 | 302 | imu_msg.angular_velocity_covariance[3] = 0; 303 | //imu_msg.angular_velocity_covariance[4] = 0.02; 304 | imu_msg.angular_velocity_covariance[4] = 0; 305 | imu_msg.angular_velocity_covariance[5] = 0; 306 | 307 | imu_msg.angular_velocity_covariance[6] = 0; 308 | imu_msg.angular_velocity_covariance[7] = 0; 309 | //imu_msg.angular_velocity_covariance[8] = 0.02; 310 | imu_msg.angular_velocity_covariance[8] = 0; 311 | 312 | // imu_msg.orientation.w = ticm20689_pkg->qw; 313 | // imu_msg.orientation.x = ticm20689_pkg->qx; 314 | // imu_msg.orientation.y = ticm20689_pkg->qy; 315 | // imu_msg.orientation.z = ticm20689_pkg->qz; 316 | 317 | pub_imu_.publish(imu_msg); 318 | 319 | delete ticm20689_pkg; 320 | } 321 | } 322 | 323 | void CoolEyeCamD1::exectu() 324 | { 325 | cam_thread_ = new boost::thread(boost::bind(&CoolEyeCamD1::img_data_stream, this)); 326 | imu_thread_ = new boost::thread(boost::bind(&CoolEyeCamD1::imu_data_stream, this)); 327 | 328 | cam_thread_->join(); 329 | imu_thread_->join(); 330 | } 331 | 332 | 333 | CoolEyeCamD1::~CoolEyeCamD1() 334 | { 335 | } 336 | 337 | 338 | -------------------------------------------------------------------------------- /sdk/src/ceros_cam_d1_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ceros_cam_d1_driver.h" 2 | int g_nCtrl = 1; 3 | 4 | int main(int argc, char**argv) 5 | { 6 | ros::init(argc, argv, "cooleye_d1"); 7 | ros::NodeHandle nh; 8 | char *path = "cecfg_std.txt"; 9 | INIT_LOG(1000, 1000, LOGMSG_LEVEL_DEBUG); 10 | 11 | int nprocs = get_nprocs(); 12 | if (0 == nprocs || 1 == nprocs) 13 | { 14 | CCpuSet::instance()->m_nCpuSetFlag = false; 15 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "The number of cores is %d, and the cores are not bound\r\n", nprocs); 16 | } 17 | else 18 | { 19 | CCpuSet::instance()->m_nCpuSetFlag = true; 20 | for (int i = 0; i < nprocs - 1; i++) 21 | { 22 | if(i >= CPU_SET_MAX) 23 | break; 24 | 25 | CCpuSet::instance()->m_aCpuPool[i] = i + 1; 26 | CCpuSet::instance()->m_nCpuPoolSize++; 27 | } 28 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, 29 | "The number of cores is %d. The binding cores are %d to %d. Core 0 is used for special purposes.\r\n", 30 | nprocs, 1, CCpuSet::instance()->m_nCpuPoolSize); 31 | } 32 | 33 | if(argv[1]) 34 | { 35 | path = argv[1]; 36 | } 37 | 38 | CoolEyeCamD1 CoolEyeCamD1RUN(nh, path); 39 | 40 | if(!CoolEyeCamD1RUN.start_cam_D1_preprocess()) 41 | std::cout << "ERROR: camera preprocess open failed" << std::endl; 42 | 43 | if(!CoolEyeCamD1RUN.start_cam_D1_capture()) 44 | std::cout << "ERROR: camera open failed" << std::endl; 45 | 46 | if(!CoolEyeCamD1RUN.start_imu_icm26089()) 47 | std::cout << "ERROR: imu open failed" << std::endl; 48 | 49 | CoolEyeCamD1RUN.exectu(); 50 | 51 | FINI_LOG(); 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /sdk/src/cetool_cali_imu.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Implementation of accelerometer calibration. 4 | * 5 | * Transform acceleration vector to true orientation, scale and offset 6 | * 7 | * ===== Model ===== 8 | * accel_corr = accel_T * (accel_raw - accel_offs) 9 | * 10 | * accel_corr[3] - fully corrected acceleration vector in body frame 11 | * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform 12 | * accel_raw[3] - raw acceleration vector 13 | * accel_offs[3] - acceleration offset vector 14 | * 15 | * ===== Calibration ===== 16 | * 17 | * Reference vectors 18 | * accel_corr_ref[6][3] = [ g 0 0 ] // nose up 19 | * | -g 0 0 | // nose down 20 | * | 0 g 0 | // left side down 21 | * | 0 -g 0 | // right side down 22 | * | 0 0 g | // on back 23 | * [ 0 0 -g ] // level 24 | * accel_raw_ref[6][3] 25 | * 26 | * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 27 | * 28 | * 6 reference vectors * 3 axes = 18 equations 29 | * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants 30 | * 31 | * Find accel_offs 32 | * 33 | * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 34 | * 35 | * Find accel_T 36 | * 37 | * 9 unknown constants 38 | * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations 39 | * 40 | * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 41 | * 42 | * Solve separate system for each row of accel_T: 43 | * 44 | * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 45 | * 46 | * A * x = b 47 | * 48 | * x = [ accel_T[0][i] ] 49 | * | accel_T[1][i] | 50 | * [ accel_T[2][i] ] 51 | * 52 | * b = [ accel_corr_ref[0][i] ] // One measurement per side is enough 53 | * | accel_corr_ref[2][i] | 54 | * [ accel_corr_ref[4][i] ] 55 | * 56 | * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 57 | * 58 | * Matrix A is common for all three systems: 59 | * A = [ a[0][0] a[0][1] a[0][2] ] 60 | * | a[2][0] a[2][1] a[2][2] | 61 | * [ a[4][0] a[4][1] a[4][2] ] 62 | * 63 | * x = A^-1 * b 64 | * 65 | * accel_T = A^-1 * g 66 | * g = 9.80665 67 | * 68 | * ===== Rotation ===== 69 | * 70 | * Calibrating using model: 71 | * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r) 72 | * 73 | * Actual correction: 74 | * accel_corr = rot * accel_T * (accel_raw - accel_offs) 75 | * 76 | * Known: accel_T_r, accel_offs_r, rot 77 | * Unknown: accel_T, accel_offs 78 | * 79 | * Solution: 80 | * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs) 81 | * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs) 82 | * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs) 83 | * => accel_T = rot^-1 * accel_T_r * rot 84 | * => accel_offs = rot^-1 * accel_offs_r 85 | * 86 | * @author Anton Babushkin 87 | */ 88 | 89 | 90 | 91 | #include 92 | #include 93 | #include 94 | #include 95 | #include 96 | #include 97 | #include 98 | 99 | #include 100 | #include 101 | 102 | #include "cedriver_usb.h" 103 | #include "cedriver_cam.h" 104 | #include "cedriver_imu.h" 105 | #include "cedriver_config.h" 106 | #include 107 | #include 108 | 109 | 110 | int mat_invert3(float src[3][3], float dst[3][3]); 111 | 112 | 113 | 114 | static void wait_for_enter_key(void) 115 | { 116 | char c; 117 | char flag = 0; 118 | 119 | while(flag == 0) 120 | { 121 | c = std::cin.get(); 122 | if('\n' == c) 123 | { 124 | flag = 1; 125 | } 126 | } 127 | 128 | } 129 | 130 | 131 | static void print_the_buff(short int * ptr) 132 | { 133 | short int x,y,z; 134 | for(int i=0;i<1024;i++) 135 | { 136 | x = *ptr; 137 | ptr++; 138 | y = *ptr; 139 | ptr++; 140 | z = *ptr; 141 | ptr++; 142 | 143 | std::cout << x <<" "<< y <<" "< 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | using namespace cv; 41 | using namespace std; 42 | 43 | static int print_help() 44 | { 45 | cout << 46 | " Given a list of chessboard images, the number of corners (nx, ny)\n" 47 | " on the chessboards, and a flag: useCalibrated for \n" 48 | " calibrated (0) or\n" 49 | " uncalibrated \n" 50 | " (1: use cvStereoCalibrate(), 2: compute fundamental\n" 51 | " matrix separately) stereo. \n" 52 | " Calibrate the cameras and display the\n" 53 | " rectified results along with the computed disparity images. \n" << endl; 54 | cout << "Usage:\n ./stereo_calib -w= -h= -s= \n" << endl; 55 | return 0; 56 | } 57 | 58 | 59 | static void 60 | StereoCalib(const vector& imagelist, Size boardSize, float squareSize, bool displayCorners = false, bool useCalibrated=true, bool showRectified=true) 61 | { 62 | if( imagelist.size() % 2 != 0 ) 63 | { 64 | cout << "Error: the image list contains odd (non-even) number of elements\n"; 65 | return; 66 | } 67 | 68 | const int maxScale = 2; 69 | // ARRAY AND VECTOR STORAGE: 70 | 71 | vector > imagePoints[2]; 72 | 73 | Size imageSize; 74 | 75 | int i, j, k, nimages = (int)imagelist.size()/2; 76 | 77 | imagePoints[0].resize(nimages); 78 | imagePoints[1].resize(nimages); 79 | 80 | vector goodImageList; 81 | 82 | for( i = j = 0; i < nimages; i++ ) 83 | { 84 | for( k = 0; k < 2; k++ ) 85 | { 86 | const string& filename = imagelist[i*2+k]; 87 | Mat img = imread(filename, 0); 88 | if(img.empty()) 89 | break; 90 | if( imageSize == Size() ) 91 | imageSize = img.size(); 92 | else if( img.size() != imageSize ) 93 | { 94 | cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n"; 95 | break; 96 | } 97 | bool found = false; 98 | vector& corners = imagePoints[k][j]; 99 | 100 | for( int scale = 1; scale <= maxScale; scale++ ) 101 | { 102 | Mat timg; 103 | if( scale == 1 ) 104 | timg = img; 105 | else 106 | resize(img, timg, Size(), scale, scale, INTER_LINEAR_EXACT); 107 | found = findChessboardCorners(timg, boardSize, corners, 108 | CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE); 109 | if( found ) 110 | { 111 | if( scale > 1 ) 112 | { 113 | Mat cornersMat(corners); 114 | cornersMat *= 1./scale; 115 | } 116 | break; 117 | } 118 | } 119 | if( displayCorners ) 120 | { 121 | cout << filename << endl; 122 | Mat cimg, cimg1; 123 | cvtColor(img, cimg, COLOR_GRAY2BGR); 124 | drawChessboardCorners(cimg, boardSize, corners, found); 125 | double sf = 640./MAX(img.rows, img.cols); 126 | resize(cimg, cimg1, Size(), sf, sf, INTER_LINEAR_EXACT); 127 | 128 | if(k == 0) 129 | { 130 | 131 | imshow("left_corners", cimg1); 132 | } 133 | else 134 | { 135 | imshow("right_corners", cimg1); 136 | } 137 | 138 | char c = (char)waitKey(1); 139 | if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit 140 | exit(-1); 141 | } 142 | else 143 | putchar('.'); 144 | if( !found ) 145 | break; 146 | cornerSubPix(img, corners, Size(11,11), Size(-1,-1), 147 | TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 148 | 30, 0.01)); 149 | } 150 | if( k == 2 ) 151 | { 152 | goodImageList.push_back(imagelist[i*2]); 153 | goodImageList.push_back(imagelist[i*2+1]); 154 | j++; 155 | } 156 | } 157 | cout << j << " pairs have been successfully detected.\n"; 158 | nimages = j; 159 | if( nimages < 2 ) 160 | { 161 | cout << "Error: too little pairs to run the calibration\n"; 162 | return; 163 | } 164 | 165 | imagePoints[0].resize(nimages); 166 | imagePoints[1].resize(nimages); 167 | 168 | 169 | vector > objectPoints; 170 | vector > objectPoints_64F; 171 | objectPoints.resize(nimages); 172 | objectPoints_64F.resize(nimages); 173 | for( i = 0; i < nimages; i++ ) 174 | { 175 | for( j = 0; j < boardSize.height; j++ ) 176 | { 177 | for( k = 0; k < boardSize.width; k++ ) 178 | { 179 | objectPoints[i].push_back(Point3f(k*squareSize, j*squareSize, 0)); 180 | objectPoints_64F[i].push_back(Point3d(k*squareSize, j*squareSize, 0)); 181 | } 182 | } 183 | } 184 | 185 | 186 | vector> imagePoints_64F[2]; 187 | imagePoints_64F[0].resize(nimages); 188 | imagePoints_64F[1].resize(nimages); 189 | for( i = 0; i < imagePoints[0].size(); i++ ) 190 | { 191 | for(j = 0; j < imagePoints[0][i].size(); j++ ) 192 | { 193 | imagePoints_64F[0][i].push_back(cv::Point2d((double)imagePoints[0][i][j].x,(double)imagePoints[0][i][j].y)); 194 | } 195 | } 196 | 197 | for( i = 0; i < imagePoints[1].size(); i++ ) 198 | { 199 | for(j = 0; j < imagePoints[1][i].size(); j++ ) 200 | { 201 | imagePoints_64F[1][i].push_back(cv::Point2d((double)imagePoints[1][i][j].x,(double)imagePoints[1][i][j].y)); 202 | } 203 | 204 | } 205 | 206 | Mat cameraMatrix[2], distCoeffs[2]; 207 | cameraMatrix[0] = initCameraMatrix2D(objectPoints,imagePoints[0],imageSize,0); 208 | cameraMatrix[1] = initCameraMatrix2D(objectPoints,imagePoints[1],imageSize,0); 209 | 210 | 211 | Mat R, T, E, F; 212 | 213 | cout << "Running stereo calibration ...\n"; 214 | 215 | // double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1], 216 | // cameraMatrix[0], distCoeffs[0], 217 | // cameraMatrix[1], distCoeffs[1], 218 | // imageSize, R, T, E, F, 219 | // CALIB_FIX_ASPECT_RATIO + 220 | // CALIB_ZERO_TANGENT_DIST + 221 | // CALIB_USE_INTRINSIC_GUESS + 222 | // CALIB_SAME_FOCAL_LENGTH + 223 | // CALIB_RATIONAL_MODEL + 224 | // CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5, 225 | // TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) ); 226 | 227 | int flag = 0; 228 | flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; 229 | flag |= cv::fisheye::CALIB_FIX_SKEW; 230 | flag |= cv::fisheye::CALIB_CHECK_COND; 231 | 232 | double rms = cv::fisheye::stereoCalibrate(objectPoints_64F, imagePoints_64F[0], imagePoints_64F[1], 233 | cameraMatrix[0], distCoeffs[0], 234 | cameraMatrix[1], distCoeffs[1], 235 | imageSize, R, T, 236 | //fisheye::CALIB_RECOMPUTE_EXTRINSIC, 237 | flag, 238 | TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-5)); 239 | 240 | 241 | 242 | 243 | cout << "done with RMS error=" << rms << endl; 244 | 245 | // CALIBRATION QUALITY CHECK 246 | // because the output fundamental matrix implicitly 247 | // includes all the output information, 248 | // we can check the quality of calibration using the 249 | // epipolar geometry constraint: m2^t*F*m1=0 250 | 251 | vector allimgpt_fish[2]; 252 | for( k = 0; k < 2; k++ ) 253 | { 254 | for( i = 0; i < nimages; i++ ) 255 | std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt_fish[k])); 256 | } 257 | F = findFundamentalMat(Mat(allimgpt_fish[0]), Mat(allimgpt_fish[1]), FM_8POINT, 0, 0); 258 | 259 | 260 | double err = 0; 261 | int npoints = 0; 262 | vector lines[2]; 263 | for( i = 0; i < nimages; i++ ) 264 | { 265 | int npt = (int)imagePoints[0][i].size(); 266 | Mat imgpt[2]; 267 | for( k = 0; k < 2; k++ ) 268 | { 269 | imgpt[k] = Mat(imagePoints[k][i]); 270 | 271 | // undistortPoints(imgpt[k], 272 | // imgpt[k], 273 | // cameraMatrix[k], 274 | // distCoeffs[k], 275 | // Mat(), 276 | // cameraMatrix[k]); 277 | 278 | cv::fisheye::undistortPoints(imgpt[k], 279 | imgpt[k], 280 | cameraMatrix[k], 281 | distCoeffs[k], 282 | Mat(), 283 | cameraMatrix[k]); 284 | 285 | computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]); 286 | } 287 | for( j = 0; j < npt; j++ ) 288 | { 289 | double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] + 290 | imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) + 291 | fabs(imagePoints[1][i][j].x*lines[0][j][0] + 292 | imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]); 293 | err += errij; 294 | } 295 | npoints += npt; 296 | } 297 | cout << "average epipolar err = " << err/npoints << endl; 298 | 299 | // save intrinsic parameters 300 | FileStorage fs("../config/intrinsics.yml", FileStorage::WRITE); 301 | if( fs.isOpened() ) 302 | { 303 | fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] << 304 | "M2" << cameraMatrix[1] << "D2" << distCoeffs[1]; 305 | fs.release(); 306 | } 307 | else 308 | cout << "Error: can not save the intrinsic parameters\n"; 309 | 310 | Mat R1, R2, P1, P2, Q; 311 | Rect validRoi[2]; 312 | 313 | // stereoRectify(cameraMatrix[0], distCoeffs[0], 314 | // cameraMatrix[1], distCoeffs[1], 315 | // imageSize, R, T, R1, R2, P1, P2, Q, 316 | // CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]); 317 | 318 | cv::fisheye::stereoRectify( cameraMatrix[0], distCoeffs[0], 319 | cameraMatrix[1], distCoeffs[1], 320 | imageSize, R, T, R1, R2, P1, P2, Q, 321 | CALIB_ZERO_DISPARITY, 322 | imageSize, 323 | 0, 324 | 1); 325 | 326 | 327 | 328 | fs.open("../config/extrinsics.yml", FileStorage::WRITE); 329 | if( fs.isOpened() ) 330 | { 331 | fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q; 332 | fs.release(); 333 | } 334 | else 335 | cout << "Error: can not save the extrinsic parameters\n"; 336 | 337 | // OpenCV can handle left-right 338 | // or up-down camera arrangements 339 | bool isVerticalStereo = fabs(P2.at(1, 3)) > fabs(P2.at(0, 3)); 340 | 341 | // COMPUTE AND DISPLAY RECTIFICATION 342 | if( !showRectified ) 343 | return; 344 | 345 | Mat rmap[2][2]; 346 | // IF BY CALIBRATED (BOUGUET'S METHOD) 347 | if( useCalibrated ) 348 | { 349 | // we already computed everything 350 | } 351 | // OR ELSE HARTLEY'S METHOD 352 | else 353 | // use intrinsic parameters of each camera, but 354 | // compute the rectification transformation directly 355 | // from the fundamental matrix 356 | { 357 | vector allimgpt[2]; 358 | for( k = 0; k < 2; k++ ) 359 | { 360 | for( i = 0; i < nimages; i++ ) 361 | std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k])); 362 | } 363 | F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0); 364 | Mat H1, H2; 365 | //stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3); 366 | stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3); 367 | 368 | R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0]; 369 | R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1]; 370 | P1 = cameraMatrix[0]; 371 | P2 = cameraMatrix[1]; 372 | } 373 | 374 | //Precompute maps for cv::remap() 375 | fisheye::initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]); 376 | fisheye::initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]); 377 | 378 | Mat canvas; 379 | double sf; 380 | int w, h; 381 | if( !isVerticalStereo ) 382 | { 383 | sf = 600./MAX(imageSize.width, imageSize.height); 384 | w = cvRound(imageSize.width*sf); 385 | h = cvRound(imageSize.height*sf); 386 | canvas.create(h, w*2, CV_8UC3); 387 | } 388 | else 389 | { 390 | sf = 300./MAX(imageSize.width, imageSize.height); 391 | w = cvRound(imageSize.width*sf); 392 | h = cvRound(imageSize.height*sf); 393 | canvas.create(h*2, w, CV_8UC3); 394 | } 395 | 396 | for( i = 0; i < nimages; i++ ) 397 | { 398 | for( k = 0; k < 2; k++ ) 399 | { 400 | Mat img = imread(goodImageList[i*2+k], 0), rimg, cimg; 401 | remap(img, rimg, rmap[k][0], rmap[k][1], INTER_LINEAR); 402 | cvtColor(rimg, cimg, COLOR_GRAY2BGR); 403 | Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h)); 404 | resize(cimg, canvasPart, canvasPart.size(), 0, 0, INTER_AREA); 405 | if( useCalibrated ) 406 | { 407 | Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf), 408 | cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf)); 409 | rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8); 410 | } 411 | } 412 | 413 | if( !isVerticalStereo ) 414 | for( j = 0; j < canvas.rows; j += 16 ) 415 | line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8); 416 | else 417 | for( j = 0; j < canvas.cols; j += 16 ) 418 | line(canvas, Point(j, 0), Point(j, canvas.rows), Scalar(0, 255, 0), 1, 8); 419 | imshow("rectified", canvas); 420 | waitKey(500); 421 | } 422 | } 423 | 424 | 425 | static bool readStringList( const string& filename, vector& l ) 426 | { 427 | l.resize(0); 428 | FileStorage fs(filename, FileStorage::READ); 429 | if( !fs.isOpened() ) 430 | return false; 431 | FileNode n = fs.getFirstTopLevelNode(); 432 | if( n.type() != FileNode::SEQ ) 433 | return false; 434 | FileNodeIterator it = n.begin(), it_end = n.end(); 435 | for( ; it != it_end; ++it ) 436 | l.push_back((string)*it); 437 | return true; 438 | } 439 | 440 | 441 | 442 | int main(int argc, char** argv) 443 | { 444 | Size boardSize; 445 | string imagelistfn; 446 | bool showRectified; 447 | cv::CommandLineParser parser(argc, 448 | argv, 449 | "{w|11|}{h|8|}{s|30.0|}{nr||}{help||}{@input|../config/stereo_calib.xml|}"); 450 | 451 | if (parser.has("help")) 452 | return print_help(); 453 | showRectified = !parser.has("nr"); 454 | imagelistfn = parser.get("@input"); 455 | boardSize.width = parser.get("w"); 456 | boardSize.height = parser.get("h"); 457 | float squareSize = parser.get("s"); 458 | if (!parser.check()) 459 | { 460 | parser.printErrors(); 461 | return 1; 462 | } 463 | vector imagelist; 464 | bool ok = readStringList(imagelistfn, imagelist); 465 | if(!ok || imagelist.empty()) 466 | { 467 | cout << "can not open " << imagelistfn << " or the string list is empty" << endl; 468 | return print_help(); 469 | } 470 | 471 | StereoCalib(imagelist, boardSize, squareSize, true, true, showRectified); 472 | 473 | 474 | return 0; 475 | } 476 | -------------------------------------------------------------------------------- /sdk/src/cetool_cali_stereo_capture_img.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "opencv2/core/core.hpp" 11 | #include "opencv2/imgproc/imgproc.hpp" 12 | #include "opencv2/highgui/highgui.hpp" 13 | #include "opencv2/calib3d/calib3d.hpp" 14 | 15 | #include "cedriver_usb.h" 16 | #include "cedriver_cam.h" 17 | #include "cedriver_imu.h" 18 | #include "cedriver_config.h" 19 | #include "cedriver_global_config.h" 20 | #include "celib_img_process.h" 21 | 22 | using namespace cv; 23 | using namespace std; 24 | 25 | pthread_t ce_cam_capture_calib_thread; 26 | bool ce_cam_capture_calib_stop_run = false; 27 | 28 | 29 | bool g_bSaveImg = false; 30 | 31 | pthread_t ce_cali_catch_enter_key_thread; 32 | bool ce_cali_catch_enter_key_stop_run = false; 33 | 34 | 35 | extern threadsafe_queue img_pkg_list_d1; 36 | 37 | 38 | static void* ce_cali_catch_enter_key(void *) 39 | { 40 | char c; 41 | while (!ce_cali_catch_enter_key_stop_run) 42 | { 43 | c = cin.get(); 44 | if ('\n' == c) 45 | { 46 | g_bSaveImg = true; 47 | cout<<"catch enter key!"< corners_left; 100 | vector corners_right; 101 | 102 | d1_img_output_pkg *img_lr_pkg; 103 | 104 | while(!ce_cam_capture_calib_stop_run) 105 | { 106 | if(!img_pkg_list_d1.try_pop(img_lr_pkg)) 107 | { 108 | usleep(1000); 109 | continue; 110 | } 111 | 112 | if(img_pkg_list_d1.size() != 0) 113 | { 114 | delete img_lr_pkg->left_img; 115 | delete img_lr_pkg->right_img; 116 | delete img_lr_pkg; 117 | continue; 118 | } 119 | 120 | 121 | // get the image 122 | memcpy(img_left.data, img_lr_pkg->left_img->data, ce_config_get_cf_img_size()); 123 | memcpy(img_right.data,img_lr_pkg->right_img->data,ce_config_get_cf_img_size()); 124 | 125 | 126 | int ret_left = findChessboardCorners(img_left, board_size, corners_left, 127 | CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);/* 提取角点 */ 128 | 129 | cvtColor(img_left, img_left_corners, COLOR_GRAY2BGR); 130 | drawChessboardCorners(img_left_corners, board_size, corners_left, ret_left); 131 | 132 | int ret_right = findChessboardCorners(img_right, board_size, corners_right, 133 | CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);/* 提取角点 */ 134 | 135 | cvtColor(img_right, img_right_corners, COLOR_GRAY2BGR); 136 | drawChessboardCorners(img_right_corners, board_size, corners_right, ret_right); 137 | 138 | ce_merge_img(result,img_left_corners,img_right_corners); 139 | 140 | //////////////////////////////////////////////// 141 | 142 | cv::imshow("result",result); 143 | 144 | cv::waitKey(1); 145 | 146 | if (g_bSaveImg && ret_left && ret_right) 147 | { 148 | cnt_save++; 149 | stringstream ss; 150 | ss << cnt_save; 151 | cv::imwrite("left"+ss.str()+".jpg",img_left); 152 | cv::imwrite("right"+ss.str()+".jpg",img_right); 153 | g_bSaveImg = false; 154 | } 155 | 156 | delete img_lr_pkg->left_img; 157 | delete img_lr_pkg->right_img; 158 | delete img_lr_pkg; 159 | } 160 | ce_cam_capture_calib_thread = 0; 161 | pthread_exit(NULL); 162 | } 163 | 164 | int ce_cam_capture_calib_init() 165 | { 166 | int temp = pthread_create(&ce_cam_capture_calib_thread, NULL, ce_cam_capture_calib, NULL); 167 | if(temp) 168 | { 169 | printf("celog: Failed to create thread show image \r\n"); 170 | return ERROR; 171 | 172 | } 173 | return SUCCESS; 174 | } 175 | 176 | void ce_cam_capture_calib_close() 177 | { 178 | ce_cam_capture_calib_stop_run = true; 179 | if(ce_cam_capture_calib_thread != 0) 180 | { 181 | pthread_join(ce_cam_capture_calib_thread,NULL); 182 | } 183 | 184 | } 185 | 186 | 187 | int g_nCtrl; 188 | 189 | void SIGINTHandler(int nSig) 190 | { 191 | printf("capture a SIGINT signal %d\n", nSig); 192 | if(0 != g_nCtrl) 193 | g_nCtrl = 0; 194 | else 195 | g_nCtrl = 1; 196 | } 197 | 198 | int main(int argc, char* argv[]) 199 | { 200 | signal(SIGINT, SIGINTHandler); 201 | 202 | ce_config_load_settings("../config/cecfg_std.txt"); 203 | 204 | // int fd = ce_imu_capture_init(); 205 | // if(fd < 0) 206 | // { 207 | // printf("celog: imu caputre error\r\n"); 208 | // } 209 | // 210 | // else 211 | // { 212 | // printf("celog: imu caputre success\r\n"); 213 | // 214 | // fd = ce_imu_showdata_init(); 215 | // if(fd < 0) 216 | // printf("celog: imu show data error\r\n"); 217 | // else 218 | // printf("celog: imu show data success\r\n"); 219 | // } 220 | 221 | 222 | int r = ce_cam_capture_init(); 223 | if(r < 0) 224 | { 225 | printf("celog: cam capture error \r\n"); 226 | } 227 | else 228 | { 229 | printf("celog: cam capture success \r\n"); 230 | 231 | r = ce_cam_preprocess_init(); 232 | if(r < 0) 233 | { 234 | printf("celog: cam preprocess error \r\n"); 235 | } 236 | else 237 | { 238 | printf("celog: cam preprocess sucess \r\n"); 239 | 240 | r = ce_cam_capture_calib_init(); 241 | if(r < 0) 242 | printf("celog: capture calib error \r\n"); 243 | else 244 | printf("celog: capture calib sucess \r\n"); 245 | } 246 | } 247 | 248 | ce_cali_catch_enter_key_init(); 249 | 250 | g_nCtrl = 1; 251 | while(g_nCtrl) 252 | { 253 | sleep(100); 254 | } 255 | 256 | ce_cali_catch_enter_key_close(); 257 | // ce_imu_capture_close(); 258 | // ce_imu_showdata_close(); 259 | 260 | ce_cam_capture_close(); 261 | ce_cam_showimg_close(); 262 | return 0; 263 | } 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | -------------------------------------------------------------------------------- /sdk/src/cpu_set.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "logmsg.h" 10 | #include "cpu_set.h" 11 | 12 | CCpuSet * & CCpuSet::instance() 13 | { 14 | static CCpuSet * _instance = NULL; 15 | if( NULL == _instance) 16 | { 17 | _instance = new CCpuSet; 18 | } 19 | return _instance; 20 | } 21 | void CCpuSet::release() 22 | { 23 | CCpuSet * & pInstance = CCpuSet::instance(); 24 | delete pInstance; 25 | pInstance = NULL; 26 | } 27 | 28 | CCpuSet::CCpuSet() 29 | { 30 | m_nCpuSetFlag = 0; 31 | m_nCpuOccNum = 0; 32 | m_nCpuPoolSize = 0; 33 | memset(m_aCpuPool, 0, sizeof(m_aCpuPool)); 34 | pthread_mutex_init(&m_Mutex, NULL); 35 | } 36 | 37 | CCpuSet::~CCpuSet() 38 | { 39 | } 40 | 41 | int CCpuSet::SetCpu(const char *pThreadName, int para) 42 | { 43 | int nRet = 0; 44 | pthread_mutex_lock(&m_Mutex); 45 | if (0 != m_nCpuSetFlag) 46 | { 47 | if (m_nCpuOccNum >= m_nCpuPoolSize) 48 | { 49 | m_nCpuOccNum = 0; 50 | } 51 | 52 | cpu_set_t mask; 53 | CPU_ZERO(&mask); 54 | int nCpuId = m_aCpuPool[m_nCpuOccNum++]; 55 | CPU_SET(nCpuId, &mask); 56 | if (sched_setaffinity(0, sizeof(mask), &mask) == -1) 57 | { 58 | nRet = -1; 59 | if (-1 != para) 60 | { 61 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_ERROR, "Set CPU[%d] affinity failed! %s%d \n", nCpuId, pThreadName, para); 62 | } 63 | else 64 | { 65 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_ERROR, "Set CPU[%d] affinity failed! %s \n", nCpuId, pThreadName); 66 | } 67 | 68 | } 69 | else 70 | { 71 | if (-1 != para) 72 | { 73 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "Set CPU[%d] affinity succeed! %s%d \n", nCpuId, pThreadName, para); 74 | } 75 | else 76 | { 77 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "Set CPU[%d] affinity succeed! %s \n", nCpuId, pThreadName); 78 | } 79 | } 80 | } 81 | pthread_mutex_unlock(&m_Mutex); 82 | 83 | return nRet; 84 | } 85 | 86 | int CCpuSet::SetCpu(int nCpuId, const char *pThreadName, int para) 87 | { 88 | int nRet = 0; 89 | pthread_mutex_lock(&m_Mutex); 90 | if (0 != m_nCpuSetFlag) 91 | { 92 | cpu_set_t mask; 93 | CPU_ZERO(&mask); 94 | 95 | CPU_SET(nCpuId, &mask); 96 | if (sched_setaffinity(0, sizeof(mask), &mask) == -1) 97 | { 98 | nRet = -1; 99 | if (-1 != para) 100 | { 101 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_ERROR, "Set CPU[%d] affinity failed! %s%d \n", nCpuId, pThreadName, para); 102 | } 103 | else 104 | { 105 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_ERROR, "Set CPU[%d] affinity failed! %s \n", nCpuId, pThreadName); 106 | } 107 | 108 | } 109 | else 110 | { 111 | if (-1 != para) 112 | { 113 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "Set CPU[%d] affinitysucceed! %s%d \n", nCpuId, pThreadName, para); 114 | } 115 | else 116 | { 117 | WRITE_LOG(LOGMSG_ALL, LOGMSG_LEVEL_INFO, "Set CPU[%d] affinitysucceed! %s \n", nCpuId, pThreadName); 118 | } 119 | } 120 | } 121 | pthread_mutex_unlock(&m_Mutex); 122 | 123 | return nRet; 124 | } 125 | 126 | 127 | -------------------------------------------------------------------------------- /sdk/src/logmsg.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "logmsg.h" 5 | 6 | 7 | LogMsg * & LogMsg::instance() 8 | { 9 | static LogMsg * _instance = NULL; 10 | if( NULL == _instance) 11 | { 12 | _instance = new LogMsg; 13 | } 14 | return _instance; 15 | } 16 | void LogMsg::release() 17 | { 18 | LogMsg * & pInstance = LogMsg::instance(); 19 | delete pInstance; 20 | pInstance = NULL; 21 | } 22 | 23 | int LogMsg::init(int nQuantity, int nSize, int nLevel) 24 | { 25 | m_nMaxFileQuantity = nQuantity; 26 | m_nMaxFileSize = nSize * 1024 * 1024; 27 | m_nLevel = nLevel; 28 | m_pLog = NULL; 29 | m_nCount = 0; 30 | m_tTime = 0; 31 | 32 | strcpy(m_cPath, "./runlog"); 33 | 34 | if(-1 == access(m_cPath, R_OK|W_OK)) 35 | { 36 | if(mkdir(m_cPath, 0755)) 37 | printf("Make log dir[%s] error\n", m_cPath); 38 | } 39 | 40 | pthread_mutex_init(&m_Mutex, NULL); 41 | 42 | return 0; 43 | } 44 | 45 | int LogMsg::fini() 46 | { 47 | } 48 | 49 | const char * LogMsg::levelStr(int level) 50 | { 51 | switch(level) { 52 | case LOGMSG_LEVEL_DEBUG: 53 | return "DEBUG"; 54 | case LOGMSG_LEVEL_INFO: 55 | return "INFO "; 56 | case LOGMSG_LEVEL_WARN: 57 | return "WARN "; 58 | case LOGMSG_LEVEL_ERROR: 59 | return "ERROR"; 60 | default: 61 | return "UNKNOW"; 62 | } 63 | } 64 | 65 | 66 | int LogMsg::writeLog(int nLogFlag, int nLevel) 67 | { 68 | if (m_nLevel > nLevel) 69 | return 0; 70 | 71 | int nRet = 0; 72 | pthread_mutex_lock(&m_Mutex); 73 | 74 | if (NULL == m_pLog) 75 | { 76 | time_t tT = time(NULL); 77 | if (m_tTime != tT) 78 | { 79 | m_tTime = tT; 80 | m_nCount = 0; 81 | } 82 | 83 | tm * stT = localtime(&tT); 84 | sprintf(m_cFileName, "%s/runlog_%04d%02d%02d%02d%02d%02d_%03d.log", 85 | m_cPath, 1900 + stT->tm_year, 1 + stT->tm_mon, stT->tm_mday, stT->tm_hour, stT->tm_min, stT->tm_sec,m_nCount++); 86 | 87 | LogFileInfo stLogFileInfo = {0}; 88 | strcpy(stLogFileInfo.cName, m_cFileName); 89 | m_vFileList.push_back(stLogFileInfo); 90 | if (m_vFileList.size() > m_nMaxFileQuantity) 91 | { 92 | vLogFile::iterator it = m_vFileList.begin(); 93 | rename(it->cName, m_cFileName); 94 | m_vFileList.erase(it); 95 | } 96 | 97 | m_pLog = fopen( m_cFileName, "w" ); 98 | } 99 | 100 | if (LOGMSG_SHELL & nLogFlag) 101 | { 102 | fprintf(stdout, m_cBuff); 103 | } 104 | 105 | if(NULL == m_pLog) 106 | { 107 | nRet = -1; 108 | goto _quit; 109 | } 110 | 111 | if (LOGMSG_FILE & nLogFlag) 112 | { 113 | fprintf(m_pLog, "%s", m_cBuff); 114 | fflush(m_pLog); 115 | } 116 | 117 | m_unLogIndex++; 118 | 119 | if (ftell(m_pLog) >= m_nMaxFileSize) 120 | { 121 | fclose(m_pLog); 122 | m_pLog = NULL; 123 | } 124 | 125 | _quit: 126 | pthread_mutex_unlock(&m_Mutex); 127 | 128 | return nRet; 129 | } 130 | 131 | -------------------------------------------------------------------------------- /tools/1_usb_driver_for_win/camD1_driver.rar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cooleyecam/cooleye_d1_linux_sdk_ros/35ae33c7ecbecbce390cd16e2b6d14aa7aa7c669/tools/1_usb_driver_for_win/camD1_driver.rar -------------------------------------------------------------------------------- /tools/2_cali_file_for_kalibr/checkerboard_11x8_3x3cm.yaml: -------------------------------------------------------------------------------- 1 | target_type: 'checkerboard' #gridtype 2 | targetCols: 11 #number of internal chessboard corners 3 | targetRows: 8 #number of internal chessboard corners 4 | rowSpacingMeters: 0.03 #size of one chessboard square [m] 5 | colSpacingMeters: 0.03 #size of one chessboard square [m] 6 | 7 | -------------------------------------------------------------------------------- /tools/2_cali_file_for_kalibr/imu0_icm20689.yaml: -------------------------------------------------------------------------------- 1 | #Accelerometers 2 | accelerometer_noise_density: 2.00e-03 #Noise density (continuous-time) 3 | accelerometer_random_walk: 3.00e-03 #Bias random walk 4 | 5 | #Gyroscopes 6 | gyroscope_noise_density: 3.00e-03 #Noise density (continuous-time) 7 | gyroscope_random_walk: 5.00e-05 #Bias random walk 8 | 9 | rostopic: /imu0_icm20689 #the IMU ROS topic 10 | update_rate: 1000.0 #Hz (for discretization of the values above) 11 | 12 | -------------------------------------------------------------------------------- /tools/3_updataTool_for_imu/FlyMcu.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cooleyecam/cooleye_d1_linux_sdk_ros/35ae33c7ecbecbce390cd16e2b6d14aa7aa7c669/tools/3_updataTool_for_imu/FlyMcu.zip -------------------------------------------------------------------------------- /tools/4_updataTool_for_camera/release.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cooleyecam/cooleye_d1_linux_sdk_ros/35ae33c7ecbecbce390cd16e2b6d14aa7aa7c669/tools/4_updataTool_for_camera/release.zip --------------------------------------------------------------------------------