├── .gitignore ├── CMakeLists.txt ├── README.md ├── cfg └── visensor_node.cfg ├── include └── visensor.hpp ├── launch ├── dense.launch └── dense_UAV.launch ├── msg ├── visensor_calibration.msg ├── visensor_imu.msg └── visensor_time_host.msg ├── package.xml ├── src ├── visensor.cpp └── visensor_node.cpp └── srv └── visensor_calibration_service.srv /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Compiled Dynamic libraries 8 | *.so 9 | *.dylib 10 | *.dll 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | *.lib 17 | 18 | # Executables 19 | *.exe 20 | *.out 21 | *.app 22 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(visensor_node) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | message_generation 7 | geometry_msgs 8 | sensor_msgs 9 | cv_bridge 10 | std_msgs 11 | image_transport 12 | camera_info_manager 13 | dynamic_reconfigure 14 | cmake_modules 15 | ) 16 | 17 | # check libvisensor version, flags not used later 18 | find_package(libvisensor 1.1.0 REQUIRED) 19 | 20 | add_message_files( 21 | DIRECTORY msg 22 | FILES visensor_imu.msg 23 | visensor_time_host.msg 24 | visensor_calibration.msg 25 | ) 26 | 27 | add_service_files( 28 | FILES 29 | visensor_calibration_service.srv 30 | ) 31 | 32 | generate_messages(DEPENDENCIES geometry_msgs) 33 | 34 | include_directories(include ${catkin_INCLUDE_DIRS} ${libvisensor_INCLUDE_DIRS}) 35 | 36 | find_package(Eigen REQUIRED) 37 | include_directories(${EIGEN_INCLUDE_DIR}) 38 | add_definitions(${EIGEN_DEFINITIONS}) 39 | 40 | find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc calib3d) 41 | 42 | generate_dynamic_reconfigure_options(cfg/visensor_node.cfg) 43 | 44 | if(NOT DEFINED CMAKE_BUILD_TYPE) 45 | set(CMAKE_BUILD_TYPE Release) 46 | endif(NOT DEFINED CMAKE_BUILD_TYPE) 47 | 48 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native -Wall -std=c++0x -D__STRICT_ANSI__") 49 | 50 | catkin_package( 51 | INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} 52 | CATKIN_DEPENDS 53 | roscpp 54 | sensor_msgs 55 | cv_bridge 56 | std_msgs 57 | image_transport 58 | camera_info_manager 59 | ) 60 | 61 | #build and add libvisensor system library dependency 62 | add_executable(visensor_node src/visensor_node.cpp src/visensor.cpp ) 63 | 64 | add_dependencies(visensor_node ${${PROJECT_NAME}_EXPORTED_TARGETS}}) 65 | target_link_libraries(visensor_node ${libvisensor_LIBRARIES} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 66 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | visensor-node 2 | ============= 3 | 4 | ROS interface to the Visual-Inertial Sensor developed by the [Autonomous Systems Lab (ASL), ETH Zurich](http://www.asl.ethz.ch) and [Skybotix AG](http://www.skybotix.com). This sensor provides fully time-synchronized and factory calibrated IMU- and stereo-camera datastreams. A detailed spec sheet of the sensor can be found [here](https://github.com/ethz-asl/libvisensor/blob/master/VISensor_Factsheet_web.pdf). The low-level driver for the VI-Sensor can be found [here](https://github.com/ethz-asl/libvisensor). 5 | 6 | ![alt text](http://wiki.ros.org/vi_sensor?action=AttachFile&do=get&target=vi-sensor-front.jpg "Sensor Photo") 7 | 8 | A detailed description on how the sensor can be used in ROS is found in the corresponding [ROS-Wiki](http://wiki.ros.org/vi_sensor). 9 | 10 | ##Authors 11 | * Michael Burri [michael.burri at mavt.ethz.ch] 12 | * Janosch Nikolic [janosch.nikolic at mavt.ethz.ch] 13 | * Jörn Rehder [joern.rehder at mavt.ethz.ch] 14 | * Stefan Leutenegger [s.leutenegger at imperial.ac.uk] 15 | * Thomas Schneider [schneith at ethz.ch] 16 | * Pascal Gohl [pascal.gohl at mavt.ethz.ch] 17 | * Sammy Omari [sammy.omari at mavt.ethz.ch] 18 | 19 | ## Reference 20 | Please cite our paper when using the VI-Sensor in an academic publication. 21 | 22 | 1. Nikolic, J., Rehder, J., Burri, M., Gohl, P., Leutenegger, S., Furgale, P. T., & Siegwart, R. (2014). *A Synchronized Visual-Inertial Sensor System with FPGA Pre-Processing for Accurate Real-Time SLAM.* IEEE International Conference on Robotics and Automation (ICRA), Hongkong, China. ( [video] (http://youtu.be/jcjB_Pflu5A) ) 23 | 24 | as bibtex: 25 | ``` 26 | @inproceedings{nikolic2014synchronized, 27 | title={A synchronized visual-inertial sensor system with FPGA pre-processing for accurate real-time SLAM}, 28 | author={Nikolic, Janosch and Rehder, Joern and Burri, Michael and Gohl, Pascal and Leutenegger, Stefan and Furgale, Paul T and Siegwart, Roland}, 29 | booktitle={Robotics and Automation (ICRA), 2014 IEEE International Conference on}, 30 | pages={431--437}, 31 | year={2014}, 32 | organization={IEEE} 33 | } 34 | ``` 35 | 36 | ## Installation 37 | 38 | Check out the sensor library and this node to your catkin workspace: 39 | 40 | ``` 41 | cd your_catkin_workspace 42 | git clone https://github.com/ethz-asl/visensor_node.git 43 | git clone https://github.com/ethz-asl/libvisensor.git 44 | ``` 45 | 46 | Make sure that you installed all necessary ROS packages 47 | 48 | ``` 49 | sudo apt-get install libeigen3-dev libopencv-dev libboost-dev ros-indigo-cmake-modules 50 | ``` 51 | Adjust the packet name to your ros version. 52 | 53 | Build the package using catkin_make 54 | 55 | ``` 56 | catkin_make 57 | ``` 58 | -------------------------------------------------------------------------------- /cfg/visensor_node.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE="visensor_node" 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | # ========================= CAMERA 0 ========================== 10 | # Name Type Level Description Default Min Max 11 | gen.add("cam0_name", str_t, 0, "camera 0", "cam0") 12 | gen.add("cam0_agc_enable", bool_t, 0, "automatic gain control", True) 13 | gen.add("cam0_max_analog_gain", int_t, 0, "max analog gain", 64, 16, 64) 14 | gen.add("cam0_global_analog_gain", int_t, 0, "analog gain", 16, 16, 64) 15 | gen.add("cam0_global_analog_gain_attenuation", bool_t, 0, "gain attenuation", False) 16 | 17 | gen.add("digital_gain", int_t, 0, "digital gain", 4, 1, 15) 18 | 19 | 20 | gen.add("cam0_aec_enable", bool_t, 0, "automatic exposure control", True) 21 | gen.add("cam0_min_coarse_shutter_width", int_t, 0, "min shutter width", 2, 2, 32765) 22 | gen.add("cam0_max_coarse_shutter_width", int_t, 0, "max shutter width", 500, 2, 32765) 23 | gen.add("cam0_coarse_shutter_width", int_t, 0, "shutter", 480, 2, 32765) 24 | gen.add("cam0_fine_shutter_width", int_t, 0, "shutter", 0, 0, 1774) 25 | 26 | gen.add("cam0_adc_mode", int_t, 0, "ADC Mode", 2,2,3) 27 | gen.add("cam0_vref_adc_voltage_level", int_t, 0, "Vref ADC Voltage level", 4,0,7) 28 | 29 | gen.add("cam0_black_level_calibration_manual_override", bool_t, 0, "Black level calibration manual override", False) 30 | gen.add("cam0_black_level_calibration_value", int_t, 0, "Black level calibration value", 0,-127,127) 31 | 32 | 33 | # ========================= CAMERA 1 ========================== 34 | 35 | gen.add("cam1_name", str_t, 0, "camera 1", "cam1") 36 | gen.add("cam1_agc_enable", bool_t, 0, "automatic gain control", True) 37 | gen.add("cam1_max_analog_gain", int_t, 0, "max analog gain", 64, 16, 64) 38 | gen.add("cam1_global_analog_gain", int_t, 0, "analog gain", 16, 16, 64) 39 | gen.add("cam1_global_analog_gain_attenuation", bool_t, 0, "gain attenuation", False) 40 | 41 | gen.add("cam1_aec_enable", bool_t, 0, "automatic exposure control", True) 42 | gen.add("cam1_min_coarse_shutter_width", int_t, 0, "min shutter width", 2, 2, 32765) 43 | gen.add("cam1_max_coarse_shutter_width", int_t, 0, "max shutter width", 500, 2, 32765) 44 | gen.add("cam1_coarse_shutter_width", int_t, 0, "shutter", 480, 2, 32765) 45 | 46 | gen.add("cam1_adc_mode", int_t, 0, "ADC Mode", 2,2,3) 47 | gen.add("cam1_vref_adc_voltage_level", int_t, 0, "Vref ADC Voltage level", 4,0,7) 48 | 49 | # ========================= CAMERA 2 ========================== 50 | 51 | gen.add("cam2_name", str_t, 0, "camera 2", "cam2") 52 | gen.add("cam2_agc_enable", bool_t, 0, "automatic gain control", True) 53 | gen.add("cam2_max_analog_gain", int_t, 0, "max analog gain", 64, 16, 64) 54 | gen.add("cam2_global_analog_gain", int_t, 0, "analog gain", 16, 16, 64) 55 | gen.add("cam2_global_analog_gain_attenuation", bool_t, 0, "gain attenuation", False) 56 | 57 | gen.add("cam2_aec_enable", bool_t, 0, "automatic exposure control", True) 58 | gen.add("cam2_min_coarse_shutter_width", int_t, 0, "min shutter width", 2, 2, 32765) 59 | gen.add("cam2_max_coarse_shutter_width", int_t, 0, "max shutter width", 500, 2, 32765) 60 | gen.add("cam2_coarse_shutter_width", int_t, 0, "shutter", 480, 2, 32765) 61 | 62 | gen.add("cam2_adc_mode", int_t, 0, "ADC Mode", 2,2,3) 63 | gen.add("cam2_vref_adc_voltage_level", int_t, 0, "Vref ADC Voltage level", 4,0,7) 64 | 65 | # ========================= CAMERA 3 ========================== 66 | 67 | gen.add("cam3_name", str_t, 0, "camera 3", "cam3") 68 | gen.add("cam3_agc_enable", bool_t, 0, "automatic gain control", True) 69 | gen.add("cam3_max_analog_gain", int_t, 0, "max analog gain", 64, 16, 64) 70 | gen.add("cam3_global_analog_gain", int_t, 0, "analog gain", 16, 16, 64) 71 | gen.add("cam3_global_analog_gain_attenuation", bool_t, 0, "gain attenuation", False) 72 | 73 | gen.add("cam3_aec_enable", bool_t, 0, "automatic exposure control", True) 74 | gen.add("cam3_min_coarse_shutter_width", int_t, 0, "min shutter width", 2, 2, 32765) 75 | gen.add("cam3_max_coarse_shutter_width", int_t, 0, "max shutter width", 500, 2, 32765) 76 | gen.add("cam3_coarse_shutter_width", int_t, 0, "shutter", 100, 2, 32765) 77 | 78 | gen.add("cam3_adc_mode", int_t, 0, "ADC Mode", 2,2,3) 79 | gen.add("cam3_vref_adc_voltage_level", int_t, 0, "Vref ADC Voltage level", 4,0,7) 80 | 81 | exit(gen.generate(PACKAGE, "visensor_node", "Driver")) 82 | 83 | -------------------------------------------------------------------------------- /include/visensor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #ifndef VISENSOR_H_ 33 | #define VISENSOR_H_ 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include "visensor_node/visensor_imu.h" 50 | #include "visensor_node/visensor_time_host.h" 51 | #include "visensor_node/visensor_calibration_service.h" 52 | #include "visensor_node/visensor_calibration.h" 53 | 54 | #include 55 | 56 | namespace visensor { 57 | 58 | static const std::string CAMERA_FRAME_NAME = "camera"; 59 | static const std::string ROS_TOPIC_NAME = "viensor/"; 60 | 61 | static const std::map ROS_CAMERA_NAMES { 62 | { SensorId::CAM0, "cam0" }, 63 | { SensorId::CAM1, "cam1" }, 64 | { SensorId::CAM2, "cam2" }, 65 | { SensorId::CAM3, "cam3" }, 66 | { SensorId::FLIR0, "tau0" }, 67 | { SensorId::FLIR1, "tau1" }, 68 | { SensorId::FLIR2, "tau2" }, 69 | { SensorId::FLIR3, "tau3" } }; 70 | static const std::map ROS_CAMERA_FRAME_NAMES { 71 | { SensorId::CAM0, "cam0" }, 72 | { SensorId::CAM1, "cam0" }, 73 | { SensorId::CAM2, "cam0" }, 74 | { SensorId::CAM3, "cam0" }, 75 | { SensorId::FLIR0, "tau0" }, 76 | { SensorId::FLIR1, "tau1" }, 77 | { SensorId::FLIR2, "tau2" }, 78 | { SensorId::FLIR3, "tau3" } }; 79 | static const std::map ROS_IMU_NAMES { 80 | { SensorId::IMU0, "imu0" }, 81 | { SensorId::IMU_CAM0, "mpu0" }, 82 | { SensorId::IMU_CAM1, "mpu1" }, 83 | { SensorId::IMU_CAM2, "mpu2" }, 84 | { SensorId::IMU_CAM3, "tau0" } }; 85 | static const std::map ROS_IMU_FRAME_NAMES { 86 | { SensorId::IMU0, "imu0" }, 87 | { SensorId::IMU_CAM0, "mpu0" }, 88 | { SensorId::IMU_CAM1, "mpu1" }, 89 | { SensorId::IMU_CAM2, "mpu2" }, 90 | { SensorId::IMU_CAM3, "tau0" } }; 91 | 92 | class ViSensor { 93 | public: 94 | ViSensor(ros::NodeHandle& nh); 95 | ~ViSensor(); 96 | 97 | void startSensors(const unsigned int cam_rate, const unsigned int imu_rate); 98 | 99 | //sensor callbacks 100 | void imuCallback(boost::shared_ptr imu_ptr, ViErrorCode error); 101 | void frameCallback(ViFrame::Ptr frame_ptr, ViErrorCode error); 102 | 103 | bool calibrationServiceCallback(visensor_node::visensor_calibration_service::Request &req, 104 | visensor_node::visensor_calibration_service::Response &res); 105 | //dynamic reconfigure callback 106 | void configCallback(visensor_node::DriverConfig &config, uint32_t level); 107 | 108 | private: 109 | void init(); 110 | bool getRosCameraConfig(const SensorId::SensorId& camera_id, sensor_msgs::CameraInfo& cam_info); 111 | bool getRosStereoCameraConfig(const SensorId::SensorId& camera_id_0, sensor_msgs::CameraInfo& cam_info_0, 112 | const SensorId::SensorId& camera_id_1, sensor_msgs::CameraInfo& cam_info_1); 113 | bool precacheViCalibration(const SensorId::SensorId& camera_id); 114 | 115 | private: 116 | ros::NodeHandle nh_; 117 | 118 | std::map nhc_; 119 | std::map itc_; 120 | std::map image_pub_; 121 | std::map cinfo_; 122 | 123 | std::map imu_pub_; 124 | std::map imu_custom_pub_; 125 | std::map calibration_pub_; 126 | 127 | ros::Publisher pub_time_host_; 128 | ros::ServiceServer calibration_service_; 129 | 130 | ViSensorDriver drv_; 131 | 132 | std::vector list_of_available_sensors_; 133 | std::vector list_of_camera_ids_; 134 | std::vector list_of_imu_ids_; 135 | 136 | std::map camera_imu_calibrations_; 137 | 138 | dynamic_reconfigure::Server dr_srv_; 139 | 140 | visensor_node::DriverConfig config_; 141 | }; 142 | 143 | } //namespace visensor 144 | 145 | #endif /* VISENSOR_H_ */ 146 | -------------------------------------------------------------------------------- /launch/dense.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/dense_UAV.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /msg/visensor_calibration.msg: -------------------------------------------------------------------------------- 1 | string cam_name #camera name which is used as base topic for publishing 2 | geometry_msgs/Pose T_IC #pose mapping a point expressed in camera frame into imu frame 3 | string dist_model #distortion model 4 | float64[] dist_coeff #distortion coefficients 5 | float64[] principal_point #principal point in pixels 6 | float64[] focal_length #focal lengths in pixels 7 | -------------------------------------------------------------------------------- /msg/visensor_imu.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Quaternion orientation 3 | geometry_msgs/Vector3 angular_velocity 4 | geometry_msgs/Vector3 linear_acceleration 5 | geometry_msgs/Vector3 magnetometer 6 | float64 pressure 7 | float64 temperature 8 | -------------------------------------------------------------------------------- /msg/visensor_time_host.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | time timestamp_host 4 | 5 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | visensor_node 4 | 1.1.1 5 | 6 | 7 | ros publisher for the vi-sensor 8 | 9 | 10 | http://www.skybotix.com 11 | Michael Burri, Pascal Gohl and Thomas Schneider 12 | Pascal Gohl 13 | Proprietary 14 | catkin 15 | 16 | roscpp 17 | sensor_msgs 18 | cv_bridge 19 | std_msgs 20 | image_transport 21 | camera_info_manager 22 | libvisensor 23 | dynamic_reconfigure 24 | geometry_msgs 25 | cmake_modules 26 | 27 | roscpp 28 | sensor_msgs 29 | cv_bridge 30 | std_msgs 31 | image_transport 32 | camera_info_manager 33 | libvisensor 34 | dynamic_reconfigure 35 | geometry_msgs 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/visensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "visensor.hpp" 39 | 40 | #define THRESHOLD_DATA_DELAY_WARNING 0.1 // in seconds 41 | 42 | namespace visensor { 43 | ViSensor::ViSensor(ros::NodeHandle& nh) 44 | : nh_(nh) { 45 | init(); 46 | } 47 | 48 | ViSensor::~ViSensor() { 49 | } 50 | 51 | void ViSensor::init() { 52 | try { 53 | drv_.init(); 54 | } catch (visensor::exceptions const &ex) { 55 | ROS_ERROR("%s", ex.what()); 56 | exit(1); 57 | } 58 | list_of_available_sensors_ = drv_.getListOfSensorIDs(); 59 | list_of_camera_ids_ = drv_.getListOfCameraIDs(); 60 | list_of_imu_ids_ = drv_.getListOfImuIDs(); 61 | 62 | std::string rootdir = ros::package::getPath("visensor_node"); 63 | std::string tempCameraInfoFileName; 64 | 65 | pub_time_host_ = nh_.advertise("time_host", -1); 66 | 67 | try { 68 | drv_.setCameraCallback(boost::bind(&ViSensor::frameCallback, this, _1, _2)); 69 | drv_.setImuCallback(boost::bind(&ViSensor::imuCallback, this, _1, _2)); 70 | drv_.setCameraCalibrationSlot(0); // 0 is factory calibration 71 | } catch (visensor::exceptions const &ex) { 72 | ROS_WARN("%s", ex.what()); 73 | } 74 | 75 | // initialize cameras 76 | for (auto camera_id : list_of_camera_ids_) { 77 | ros::NodeHandle nhc_temp(nh_, ROS_CAMERA_NAMES.at(camera_id)); 78 | nhc_.insert(std::pair(camera_id, nhc_temp)); 79 | image_transport::ImageTransport itc_temp(nhc_[camera_id]); 80 | itc_.insert(std::pair(camera_id, itc_temp)); 81 | ROS_INFO("Register publisher for camera %u with topic name %s", camera_id, 82 | ROS_CAMERA_NAMES.at(camera_id).c_str()); 83 | image_transport::CameraPublisher image_pub = (itc_.at(camera_id)).advertiseCamera("image_raw", 100); 84 | image_pub_.insert(std::pair(camera_id, image_pub)); 85 | 86 | precacheViCalibration(camera_id); 87 | 88 | sensor_msgs::CameraInfo cinfo_temp; 89 | if (getRosCameraConfig(camera_id, cinfo_temp)) 90 | ROS_INFO_STREAM("Read calibration for "<< ROS_CAMERA_NAMES.at(camera_id)); 91 | else 92 | ROS_INFO_STREAM("Could not read calibration for "<< ROS_CAMERA_NAMES.at(camera_id)); 93 | 94 | cinfo_.insert(std::pair(camera_id, cinfo_temp)); 95 | 96 | ros::Publisher temp_pub; 97 | temp_pub = nhc_temp.advertise("calibration", -1); 98 | calibration_pub_.insert(std::pair(camera_id, temp_pub)); 99 | } 100 | 101 | 102 | if (!drv_.isStereoCameraFlipped()){ 103 | //Generate Stereo ROS config, assuming than cam0 and cam1 are in fronto-parallel stereo configuration 104 | if (getRosStereoCameraConfig(SensorId::CAM0, cinfo_.at(SensorId::CAM0), 105 | SensorId::CAM1, cinfo_.at(SensorId::CAM1))) 106 | ROS_INFO("Generated ROS Stereo Calibration, assuming cam0 (left) and cam1 (right) are a stereo pair."); 107 | else 108 | ROS_INFO("Could not read stereo calibration for cam0 and cam1."); 109 | } 110 | else{ 111 | //Generate Stereo ROS config, assuming than cam0 and cam1 are in fronto-parallel stereo configuration 112 | if (getRosStereoCameraConfig(SensorId::CAM1, cinfo_.at(SensorId::CAM1), 113 | SensorId::CAM0, cinfo_.at(SensorId::CAM0))) 114 | ROS_INFO("Generated ROS Stereo Calibration, assuming cam1 (left) and cam0 (right) are a stereo pair."); 115 | else 116 | ROS_INFO("Could not read stereo calibration for cam0 and cam1."); 117 | } 118 | 119 | // Initialize imus 120 | for (auto imu_id : list_of_imu_ids_) { 121 | ros::Publisher temp_pub; 122 | temp_pub = nh_.advertise(ROS_IMU_NAMES.at(imu_id), -1); 123 | printf("register publisher for imu %u\n", imu_id); 124 | imu_pub_.insert(std::pair(imu_id, temp_pub)); 125 | temp_pub = nh_.advertise("cust_" + ROS_IMU_NAMES.at(imu_id), -1); 126 | imu_custom_pub_.insert(std::pair(imu_id, temp_pub)); 127 | } 128 | 129 | calibration_service_ = nh_.advertiseService("get_camera_calibration", &ViSensor::calibrationServiceCallback, this); 130 | 131 | // init dynamic reconfigure 132 | dr_srv_.setCallback(boost::bind(&ViSensor::configCallback, this, _1, _2)); 133 | } 134 | 135 | void ViSensor::startSensors(const unsigned int cam_rate, const unsigned int imu_rate) { 136 | drv_.startAllCameras(cam_rate); 137 | drv_.startAllImus(imu_rate); 138 | } 139 | 140 | void ViSensor::imuCallback(boost::shared_ptr imu_ptr, ViErrorCode error) { 141 | if (error == ViErrorCodes::MEASUREMENT_DROPPED) { 142 | ROS_WARN("dropped imu measurement on sensor %u (check network bandwidth/sensor rate)", imu_ptr->imu_id); 143 | return; 144 | } 145 | 146 | const double sigma2_gyr_adis16375_d = 0; 147 | const double sigma2_acc_adis16375_d = 0; 148 | 149 | ros::Time msg_time; 150 | msg_time.fromNSec(imu_ptr->timestamp); 151 | 152 | sensor_msgs::Imu imu_msg; 153 | 154 | imu_msg.header.stamp = msg_time; 155 | imu_msg.header.frame_id = ROS_IMU_FRAME_NAMES.at(static_cast(imu_ptr->imu_id)); 156 | imu_msg.orientation.x = 0.0; 157 | imu_msg.orientation.y = 0.0; 158 | imu_msg.orientation.z = 0.0; 159 | imu_msg.orientation.w = 1.0; 160 | imu_msg.orientation_covariance[0] = 99999.9; 161 | imu_msg.orientation_covariance[1] = 0.0; 162 | imu_msg.orientation_covariance[2] = 0.0; 163 | imu_msg.orientation_covariance[3] = 0.0; 164 | imu_msg.orientation_covariance[4] = 99999.9; 165 | imu_msg.orientation_covariance[5] = 0.0; 166 | imu_msg.orientation_covariance[6] = 0.0; 167 | imu_msg.orientation_covariance[7] = 0.0; 168 | imu_msg.orientation_covariance[8] = 99999.9; 169 | // --- Angular Velocity. 170 | imu_msg.angular_velocity.x = imu_ptr->gyro[0]; 171 | imu_msg.angular_velocity.y = imu_ptr->gyro[1]; 172 | imu_msg.angular_velocity.z = imu_ptr->gyro[2]; 173 | imu_msg.angular_velocity_covariance[0] = sigma2_gyr_adis16375_d; 174 | imu_msg.angular_velocity_covariance[1] = 0.0; 175 | imu_msg.angular_velocity_covariance[2] = 0.0; 176 | imu_msg.angular_velocity_covariance[3] = 0.0; 177 | imu_msg.angular_velocity_covariance[4] = sigma2_gyr_adis16375_d; 178 | imu_msg.angular_velocity_covariance[5] = 0.0; 179 | imu_msg.angular_velocity_covariance[6] = 0.0; 180 | imu_msg.angular_velocity_covariance[7] = 0.0; 181 | imu_msg.angular_velocity_covariance[8] = sigma2_gyr_adis16375_d; 182 | // --- Linear Acceleration. 183 | imu_msg.linear_acceleration.x = imu_ptr->acc[0]; 184 | imu_msg.linear_acceleration.y = imu_ptr->acc[1]; 185 | imu_msg.linear_acceleration.z = imu_ptr->acc[2]; 186 | imu_msg.linear_acceleration_covariance[0] = sigma2_acc_adis16375_d; 187 | imu_msg.linear_acceleration_covariance[1] = 0.0; 188 | imu_msg.linear_acceleration_covariance[2] = 0.0; 189 | imu_msg.linear_acceleration_covariance[3] = 0.0; 190 | imu_msg.linear_acceleration_covariance[4] = sigma2_acc_adis16375_d; 191 | imu_msg.linear_acceleration_covariance[5] = 0.0; 192 | imu_msg.linear_acceleration_covariance[6] = 0.0; 193 | imu_msg.linear_acceleration_covariance[7] = 0.0; 194 | imu_msg.linear_acceleration_covariance[8] = sigma2_acc_adis16375_d; 195 | 196 | visensor_node::visensor_imu imu2; 197 | imu2.header.stamp = msg_time; 198 | imu2.header.frame_id = ROS_IMU_FRAME_NAMES.at(static_cast(imu_ptr->imu_id)); 199 | imu2.header.seq = 5; 200 | 201 | imu2.angular_velocity.x = imu_ptr->gyro[0]; 202 | imu2.angular_velocity.y = imu_ptr->gyro[1]; 203 | imu2.angular_velocity.z = imu_ptr->gyro[2]; 204 | 205 | imu2.linear_acceleration.x = imu_ptr->acc[0]; 206 | imu2.linear_acceleration.y = imu_ptr->acc[1]; 207 | imu2.linear_acceleration.z = imu_ptr->acc[2]; 208 | 209 | imu2.magnetometer.x = imu_ptr->mag[0]; 210 | imu2.magnetometer.y = imu_ptr->mag[1]; 211 | imu2.magnetometer.z = imu_ptr->mag[2]; 212 | 213 | imu2.pressure = imu_ptr->baro; 214 | imu2.temperature = imu_ptr->temperature; 215 | 216 | // --- Publish IMU Message. 217 | imu_pub_.at(static_cast(imu_ptr->imu_id)).publish(imu_msg); 218 | 219 | // --- Publish custom IMU Message. 220 | imu_custom_pub_.at(static_cast(imu_ptr->imu_id)).publish(imu2); 221 | } 222 | 223 | void ViSensor::frameCallback(ViFrame::Ptr frame_ptr, ViErrorCode error) { 224 | if (error == ViErrorCodes::MEASUREMENT_DROPPED) { 225 | ROS_WARN("dropped camera image on sensor %u (check network bandwidth/sensor rate)", frame_ptr->camera_id); 226 | return; 227 | } 228 | 229 | int image_height = frame_ptr->height; 230 | int image_width = frame_ptr->width; 231 | 232 | // get sensor time of message 233 | ros::Time msg_time; 234 | msg_time.fromNSec(frame_ptr->timestamp); 235 | 236 | // check if transmission is delayed 237 | const double frame_delay = (ros::Time::now() - msg_time).toSec(); 238 | if (frame_delay > THRESHOLD_DATA_DELAY_WARNING) 239 | ROS_WARN("Data arrived later than expected [ms]: %f", frame_delay * 1000.0); 240 | 241 | // get system time of message 242 | ros::Time msg_time_host; 243 | msg_time_host.fromNSec(frame_ptr->timestamp_host); 244 | 245 | // create new time message 246 | visensor_node::visensor_time_host time_msg; 247 | time_msg.header.stamp = msg_time; 248 | time_msg.timestamp_host = msg_time_host; 249 | pub_time_host_.publish(time_msg); 250 | 251 | // create new image message 252 | sensor_msgs::Image msg; 253 | msg.header.stamp = msg_time; 254 | msg.header.frame_id = ROS_CAMERA_FRAME_NAMES.at(static_cast(frame_ptr->camera_id)); 255 | 256 | if (frame_ptr->image_type == MONO8) 257 | sensor_msgs::fillImage(msg, sensor_msgs::image_encodings::MONO8, image_height, image_width, image_width, 258 | frame_ptr->getImageRawPtr()); 259 | else if (frame_ptr->image_type == MONO16) { 260 | cv::Mat image; 261 | image.create(image_height, image_width, CV_16UC1); 262 | 263 | cv::Mat image_8bit; 264 | image_8bit.create(image_height, image_width, CV_8UC1); 265 | 266 | memcpy(image.data, frame_ptr->getImageRawPtr(), (image_width) * image_height * 2); 267 | 268 | sensor_msgs::fillImage(msg, sensor_msgs::image_encodings::MONO16, image_height, image_width, image_width * 2, 269 | image.data); 270 | } else 271 | ROS_WARN("[VI_SENSOR] - unknown image type!"); 272 | 273 | // get current CameraInfo data 274 | sensor_msgs::CameraInfo ci = cinfo_[static_cast(frame_ptr->camera_id)]; 275 | 276 | // fill header 277 | ci.header.frame_id = ROS_CAMERA_FRAME_NAMES.at(static_cast(frame_ptr->camera_id)); 278 | ci.header.stamp = msg_time; 279 | 280 | ci.height = image_height; 281 | ci.width = image_width; 282 | 283 | // publish image 284 | image_pub_[static_cast(frame_ptr->camera_id)].publish(msg, ci); 285 | 286 | calibration_pub_[static_cast(frame_ptr->camera_id)].publish( 287 | camera_imu_calibrations_[ROS_CAMERA_NAMES.at(static_cast(frame_ptr->camera_id))]); 288 | } 289 | 290 | void ViSensor::configCallback(visensor_node::DriverConfig &config, uint32_t level) { 291 | 292 | std::vector all_available_sensor_ids = drv_.getListOfSensorIDs(); 293 | 294 | // configure MPU 9150 IMU (if available) 295 | if (std::count(list_of_imu_ids_.begin(), list_of_imu_ids_.end(), visensor::SensorId::IMU_CAM0) > 0) 296 | drv_.setSensorConfigParam(visensor::SensorId::IMU_CAM0, "digital_low_pass_filter_config", 0); 297 | 298 | if (std::count(list_of_imu_ids_.begin(), list_of_imu_ids_.end(), visensor::SensorId::IMU_CAM1) > 0) 299 | drv_.setSensorConfigParam(visensor::SensorId::IMU_CAM1, "digital_low_pass_filter_config", 0); 300 | 301 | // ========================= CAMERA 0 ========================== 302 | 303 | if (std::count(list_of_camera_ids_.begin(), list_of_camera_ids_.end(), visensor::SensorId::CAM0) > 0) { 304 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "agc_enable", config.cam0_agc_enable); 305 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "max_analog_gain", config.cam0_max_analog_gain); 306 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "global_analog_gain", config.cam0_global_analog_gain); 307 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "global_analog_gain_attenuation", 308 | config.cam0_global_analog_gain_attenuation); 309 | 310 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "aec_enable", config.cam0_aec_enable); 311 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "min_coarse_shutter_width", 312 | config.cam0_min_coarse_shutter_width); 313 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "max_coarse_shutter_width", 314 | config.cam0_max_coarse_shutter_width); 315 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "coarse_shutter_width", config.cam0_coarse_shutter_width); 316 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "fine_shutter_width", config.cam0_fine_shutter_width); 317 | 318 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "adc_mode", config.cam0_adc_mode); 319 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "vref_adc_voltage_level", 320 | config.cam0_vref_adc_voltage_level); 321 | 322 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "black_level_calibration_manual_override", 323 | config.cam0_black_level_calibration_manual_override); 324 | if (config.cam0_black_level_calibration_manual_override) 325 | drv_.setSensorConfigParam(visensor::SensorId::CAM0, "black_level_calibration_value", 326 | config.cam0_black_level_calibration_value); 327 | 328 | } 329 | 330 | // ========================= CAMERA 1 ========================== 331 | if (std::count(list_of_camera_ids_.begin(), list_of_camera_ids_.end(), visensor::SensorId::CAM1) > 0) { 332 | drv_.setSensorConfigParam(visensor::SensorId::CAM1, "agc_enable", config.cam1_agc_enable); 333 | drv_.setSensorConfigParam(visensor::SensorId::CAM1, "max_analog_gain", config.cam1_max_analog_gain); 334 | drv_.setSensorConfigParam(visensor::SensorId::CAM1, "global_analog_gain", config.cam1_global_analog_gain); 335 | drv_.setSensorConfigParam(visensor::SensorId::CAM1, "global_analog_gain_attenuation", 336 | config.cam1_global_analog_gain_attenuation); 337 | 338 | drv_.setSensorConfigParam(visensor::SensorId::CAM1, "aec_enable", config.cam1_aec_enable); 339 | drv_.setSensorConfigParam(visensor::SensorId::CAM1, "min_coarse_shutter_width", 340 | config.cam1_min_coarse_shutter_width); 341 | drv_.setSensorConfigParam(visensor::SensorId::CAM1, "max_coarse_shutter_width", 342 | config.cam1_max_coarse_shutter_width); 343 | drv_.setSensorConfigParam(visensor::SensorId::CAM1, "coarse_shutter_width", config.cam1_coarse_shutter_width); 344 | 345 | drv_.setSensorConfigParam(visensor::SensorId::CAM1, "adc_mode", config.cam1_adc_mode); 346 | drv_.setSensorConfigParam(visensor::SensorId::CAM1, "vref_adc_voltage_level", 347 | config.cam1_vref_adc_voltage_level); 348 | } 349 | 350 | // ========================= CAMERA 2 ========================== 351 | if (std::count(list_of_camera_ids_.begin(), list_of_camera_ids_.end(), visensor::SensorId::CAM2) > 0) { 352 | drv_.setSensorConfigParam(visensor::SensorId::CAM2, "agc_enable", config.cam2_agc_enable); 353 | drv_.setSensorConfigParam(visensor::SensorId::CAM2, "max_analog_gain", config.cam2_max_analog_gain); 354 | drv_.setSensorConfigParam(visensor::SensorId::CAM2, "global_analog_gain", config.cam2_global_analog_gain); 355 | drv_.setSensorConfigParam(visensor::SensorId::CAM2, "global_analog_gain_attenuation", 356 | config.cam2_global_analog_gain_attenuation); 357 | 358 | drv_.setSensorConfigParam(visensor::SensorId::CAM2, "aec_enable", config.cam2_aec_enable); 359 | drv_.setSensorConfigParam(visensor::SensorId::CAM2, "min_coarse_shutter_width", 360 | config.cam2_min_coarse_shutter_width); 361 | drv_.setSensorConfigParam(visensor::SensorId::CAM2, "max_coarse_shutter_width", 362 | config.cam2_max_coarse_shutter_width); 363 | drv_.setSensorConfigParam(visensor::SensorId::CAM2, "coarse_shutter_width", config.cam2_coarse_shutter_width); 364 | 365 | drv_.setSensorConfigParam(visensor::SensorId::CAM2, "adc_mode", config.cam2_adc_mode); 366 | drv_.setSensorConfigParam(visensor::SensorId::CAM2, "vref_adc_voltage_level", 367 | config.cam2_vref_adc_voltage_level); 368 | } 369 | 370 | // ========================= CAMERA 3 ========================== 371 | if (std::count(list_of_camera_ids_.begin(), list_of_camera_ids_.end(), visensor::SensorId::CAM3) > 0) { 372 | drv_.setSensorConfigParam(visensor::SensorId::CAM3, "agc_enable", config.cam3_agc_enable); 373 | drv_.setSensorConfigParam(visensor::SensorId::CAM3, "max_analog_gain", config.cam3_max_analog_gain); 374 | drv_.setSensorConfigParam(visensor::SensorId::CAM3, "global_analog_gain", config.cam3_global_analog_gain); 375 | drv_.setSensorConfigParam(visensor::SensorId::CAM3, "global_analog_gain_attenuation", 376 | config.cam3_global_analog_gain_attenuation); 377 | 378 | drv_.setSensorConfigParam(visensor::SensorId::CAM3, "aec_enable", config.cam3_aec_enable); 379 | drv_.setSensorConfigParam(visensor::SensorId::CAM3, "min_coarse_shutter_width", 380 | config.cam3_min_coarse_shutter_width); 381 | drv_.setSensorConfigParam(visensor::SensorId::CAM3, "max_coarse_shutter_width", 382 | config.cam3_max_coarse_shutter_width); 383 | drv_.setSensorConfigParam(visensor::SensorId::CAM3, "coarse_shutter_width", config.cam3_coarse_shutter_width); 384 | 385 | drv_.setSensorConfigParam(visensor::SensorId::CAM3, "adc_mode", config.cam3_adc_mode); 386 | drv_.setSensorConfigParam(visensor::SensorId::CAM3, "vref_adc_voltage_level", 387 | config.cam3_vref_adc_voltage_level); 388 | 389 | } 390 | } 391 | 392 | bool ViSensor::calibrationServiceCallback(visensor_node::visensor_calibration_service::Request &req, 393 | visensor_node::visensor_calibration_service::Response &res) 394 | { 395 | for (auto i : camera_imu_calibrations_) 396 | res.calibration.push_back(i.second); 397 | return true; 398 | } 399 | 400 | bool ViSensor::getRosCameraConfig(const SensorId::SensorId& camera_id, sensor_msgs::CameraInfo& cam_info) { 401 | //TODO(omaris) Remove hardcoded vals. 402 | int image_width = 752; 403 | int image_height = 480; 404 | 405 | ViCameraCalibration camera_calibration; 406 | if (!drv_.getCameraCalibration(camera_id, camera_calibration)) 407 | return false; 408 | 409 | double c[9]; 410 | double d[5]; 411 | 412 | d[0] = camera_calibration.dist_coeff[0]; 413 | d[1] = camera_calibration.dist_coeff[1]; 414 | d[2] = camera_calibration.dist_coeff[2]; 415 | d[3] = camera_calibration.dist_coeff[3]; 416 | d[4] = 0.0; 417 | c[0] = camera_calibration.focal_point[0]; 418 | c[1] = 0.0; 419 | c[2] = camera_calibration.principal_point[0]; 420 | c[3] = 0.0; 421 | c[4] = camera_calibration.focal_point[1]; 422 | c[5] = camera_calibration.principal_point[1]; 423 | c[6] = 0.0; 424 | c[7] = 0.0; 425 | c[8] = 1.0; 426 | 427 | if (cam_info.D.size() != 5) 428 | cam_info.D.resize(5); 429 | 430 | for (int i = 0; i < 5; i++) { 431 | cam_info.D[i] = d[i]; 432 | } 433 | 434 | for (int i = 0; i < 9; i++) { 435 | cam_info.K[i] = c[i]; 436 | } 437 | 438 | cam_info.width = image_width; 439 | cam_info.height = image_height; 440 | 441 | cam_info.binning_x = 1; 442 | cam_info.binning_y = 1; 443 | 444 | cam_info.distortion_model = std::string("plumb_bob"); 445 | 446 | return true; 447 | } 448 | 449 | bool ViSensor::getRosStereoCameraConfig(const SensorId::SensorId& camera_id_0, 450 | sensor_msgs::CameraInfo& cam_info_0, 451 | const SensorId::SensorId& camera_id_1, 452 | sensor_msgs::CameraInfo& cam_info_1) { 453 | ViCameraCalibration camera_calibration_0, camera_calibration_1; 454 | 455 | if (!drv_.getCameraCalibration(camera_id_0, camera_calibration_0)) 456 | return false; 457 | if (!drv_.getCameraCalibration(camera_id_1, camera_calibration_1)) 458 | return false; 459 | 460 | //TODO(omaris) Remove hardcoded vals. 461 | int image_width = 752; 462 | int image_height = 480; 463 | 464 | double c0[9]; 465 | double d0[5]; 466 | double r0[9]; 467 | double p0[12]; 468 | double rot0[9]; 469 | double t0[3]; 470 | 471 | double c1[9]; 472 | double d1[5]; 473 | double r1[9]; 474 | double p1[12]; 475 | double rot1[9]; 476 | double t1[3]; 477 | 478 | double r[9]; 479 | double t[3]; 480 | 481 | d0[0] = camera_calibration_0.dist_coeff[0]; 482 | d0[1] = camera_calibration_0.dist_coeff[1]; 483 | d0[2] = camera_calibration_0.dist_coeff[2]; 484 | d0[3] = camera_calibration_0.dist_coeff[3]; 485 | d0[4] = 0.0; 486 | c0[0] = camera_calibration_0.focal_point[0]; 487 | c0[1] = 0.0; 488 | c0[2] = camera_calibration_0.principal_point[0]; 489 | c0[3] = 0.0; 490 | c0[4] = camera_calibration_0.focal_point[1]; 491 | c0[5] = camera_calibration_0.principal_point[1]; 492 | c0[6] = 0.0; 493 | c0[7] = 0.0; 494 | c0[8] = 1.0; 495 | 496 | d1[0] = camera_calibration_1.dist_coeff[0]; 497 | d1[1] = camera_calibration_1.dist_coeff[1]; 498 | d1[2] = camera_calibration_1.dist_coeff[2]; 499 | d1[3] = camera_calibration_1.dist_coeff[3]; 500 | d1[4] = 0.0; 501 | c1[0] = camera_calibration_1.focal_point[0]; 502 | c1[1] = 0.0; 503 | c1[2] = camera_calibration_1.principal_point[0]; 504 | c1[3] = 0.0; 505 | c1[4] = camera_calibration_1.focal_point[1]; 506 | c1[5] = camera_calibration_1.principal_point[1]; 507 | c1[6] = 0.0; 508 | c1[7] = 0.0; 509 | c1[8] = 1.0; 510 | 511 | for (int i = 0; i < 9; ++i) { 512 | rot0[i] = camera_calibration_0.R[i]; 513 | rot1[i] = camera_calibration_1.R[i]; 514 | } 515 | for (int i = 0; i < 3; ++i) { 516 | t0[i] = camera_calibration_0.t[i]; 517 | t1[i] = camera_calibration_1.t[i]; 518 | } 519 | 520 | Eigen::Map < Eigen::Matrix3d > RR0(rot0); 521 | Eigen::Map < Eigen::Vector3d > tt0(t0); 522 | Eigen::Map < Eigen::Matrix3d > RR1(rot1); 523 | Eigen::Map < Eigen::Vector3d > tt1(t1); 524 | 525 | Eigen::Matrix4d T0 = Eigen::Matrix4d::Zero(); 526 | Eigen::Matrix4d T1 = Eigen::Matrix4d::Zero(); 527 | 528 | T0.block<3, 3>(0, 0) = RR0; 529 | T0.block<3, 1>(0, 3) = tt0; 530 | T0(3, 3) = 1.0; 531 | T1.block<3, 3>(0, 0) = RR1; 532 | T1.block<3, 1>(0, 3) = tt1; 533 | T1(3, 3) = 1.0; 534 | 535 | Eigen::Matrix4d T_rel = Eigen::Matrix4d::Zero(); 536 | T_rel = T1 * T0.inverse(); 537 | 538 | Eigen::Map < Eigen::Matrix3d > R_rel(r); 539 | Eigen::Map < Eigen::Vector3d > t_rel(t); 540 | 541 | R_rel = T_rel.block<3, 3>(0, 0); 542 | t_rel << T_rel(0, 3), T_rel(1, 3), T_rel(2, 3); 543 | 544 | double r_temp[9]; 545 | r_temp[0] = R_rel(0, 0); 546 | r_temp[1] = R_rel(0, 1); 547 | r_temp[2] = R_rel(0, 2); 548 | r_temp[3] = R_rel(1, 0); 549 | r_temp[4] = R_rel(1, 1); 550 | r_temp[5] = R_rel(1, 2); 551 | r_temp[6] = R_rel(2, 0); 552 | r_temp[7] = R_rel(2, 1); 553 | r_temp[8] = R_rel(2, 2); 554 | 555 | //cv::Mat wrapped(rows, cols, CV_32FC1, external_mem, CV_AUTOSTEP); 556 | cv::Mat C0(3, 3, CV_64FC1, c0, 3 * sizeof(double)); 557 | cv::Mat D0(5, 1, CV_64FC1, d0, 1 * sizeof(double)); 558 | cv::Mat R0(3, 3, CV_64FC1, r0, 3 * sizeof(double)); 559 | cv::Mat P0(3, 4, CV_64FC1, p0, 4 * sizeof(double)); 560 | 561 | cv::Mat C1(3, 3, CV_64FC1, c1, 3 * sizeof(double)); 562 | cv::Mat D1(5, 1, CV_64FC1, d1, 1 * sizeof(double)); 563 | cv::Mat R1(3, 3, CV_64FC1, r1, 3 * sizeof(double)); 564 | cv::Mat P1(3, 4, CV_64FC1, p1, 4 * sizeof(double)); 565 | 566 | cv::Mat R(3, 3, CV_64FC1, r_temp, 3 * sizeof(double)); 567 | 568 | cv::Mat T(3, 1, CV_64FC1, t, 1 * sizeof(double)); 569 | 570 | cv::Size img_size(image_width, image_height); 571 | 572 | cv::Rect roi1, roi2; 573 | cv::Mat Q; 574 | 575 | cv::stereoRectify(C0, D0, C1, D1, img_size, R, T, R0, R1, P0, P1, Q, cv::CALIB_ZERO_DISPARITY, 0, 576 | img_size, &roi1, &roi2); 577 | 578 | if (cam_info_0.D.size() != 5) 579 | cam_info_0.D.resize(5); 580 | 581 | if (cam_info_1.D.size() != 5) 582 | cam_info_1.D.resize(5); 583 | 584 | for (int i = 0; i < 5; i++) { 585 | cam_info_0.D[i] = d0[i]; 586 | cam_info_1.D[i] = d1[i]; 587 | } 588 | for (int i = 0; i < 9; i++) { 589 | cam_info_0.K[i] = c0[i]; 590 | cam_info_0.R[i] = R0.at(i); 591 | cam_info_1.K[i] = c1[i]; 592 | cam_info_1.R[i] = R1.at(i); 593 | } 594 | for (int i = 0; i < 12; i++) { 595 | cam_info_0.P[i] = P0.at(i); 596 | cam_info_1.P[i] = P1.at(i); 597 | } 598 | cam_info_0.width = image_width; 599 | cam_info_1.width = image_width; 600 | 601 | cam_info_0.height = image_height; 602 | cam_info_1.height = image_height; 603 | 604 | cam_info_0.binning_x = 1; 605 | cam_info_0.binning_y = 1; 606 | cam_info_1.binning_x = 1; 607 | cam_info_1.binning_y = 1; 608 | 609 | cam_info_0.distortion_model = std::string("plumb_bob"); 610 | cam_info_1.distortion_model = std::string("plumb_bob"); 611 | return true; 612 | } 613 | 614 | bool ViSensor::precacheViCalibration(const SensorId::SensorId& camera_id) { 615 | 616 | ViCameraCalibration camera_calibration; 617 | visensor_node::visensor_calibration calibration; 618 | 619 | geometry_msgs::Pose T_IC; 620 | if (!drv_.getCameraCalibration(camera_id, camera_calibration)) { 621 | camera_imu_calibrations_.insert(std::pair(ROS_CAMERA_NAMES.at(camera_id), calibration)); 622 | return false; 623 | } 624 | tf::Matrix3x3 R_IC(camera_calibration.R[0], camera_calibration.R[3], camera_calibration.R[6], 625 | camera_calibration.R[1], camera_calibration.R[4], camera_calibration.R[7], 626 | camera_calibration.R[2], camera_calibration.R[5], camera_calibration.R[8]); 627 | 628 | tf::Quaternion q_IC; 629 | R_IC.getRotation(q_IC); 630 | 631 | T_IC.orientation.x = q_IC.x(); 632 | T_IC.orientation.y = q_IC.y(); 633 | T_IC.orientation.z = q_IC.z(); 634 | T_IC.orientation.w = q_IC.w(); 635 | 636 | T_IC.position.x = camera_calibration.t[0]; 637 | T_IC.position.y = camera_calibration.t[1]; 638 | T_IC.position.z = camera_calibration.t[2]; 639 | 640 | calibration.T_IC = T_IC; 641 | calibration.dist_model = std::string("plumb_bob"); 642 | calibration.dist_coeff.push_back(camera_calibration.dist_coeff[0]); 643 | calibration.dist_coeff.push_back(camera_calibration.dist_coeff[1]); 644 | calibration.dist_coeff.push_back(camera_calibration.dist_coeff[2]); 645 | calibration.dist_coeff.push_back(camera_calibration.dist_coeff[3]); 646 | calibration.dist_coeff.push_back(camera_calibration.dist_coeff[4]); 647 | 648 | calibration.principal_point.push_back(camera_calibration.principal_point[0]); 649 | calibration.principal_point.push_back(camera_calibration.principal_point[1]); 650 | 651 | calibration.focal_length.push_back(camera_calibration.focal_point[0]); 652 | calibration.focal_length.push_back(camera_calibration.focal_point[1]); 653 | 654 | calibration.cam_name = ROS_CAMERA_NAMES.at(camera_id); 655 | camera_imu_calibrations_.insert(std::pair(ROS_CAMERA_NAMES.at(camera_id), calibration)); 656 | return true; 657 | } 658 | 659 | } //namespace visensor 660 | -------------------------------------------------------------------------------- /src/visensor_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com) 3 | * Copyright (c) 2014, Autonomous Systems Lab, ETH Zurich, Switzerland 4 | * 5 | * All rights reserved. 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | */ 31 | #include "visensor.hpp" 32 | 33 | int main(int argc, char** argv) { 34 | 35 | ros::init(argc, argv, "visensor_node"); 36 | ros::NodeHandle nh; 37 | 38 | //default sensor rates 39 | int cam_rate; 40 | int imu_rate ; 41 | 42 | //Read values from ROS or set to default value 43 | nh.param("imuRate", imu_rate, IMU_FREQUENCY); 44 | nh.param("camRate", cam_rate, CAMERA_FREQUENCY); 45 | 46 | visensor::ViSensor vi_sensor(nh); 47 | vi_sensor.startSensors(cam_rate, imu_rate); 48 | 49 | ros::spin(); 50 | 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /srv/visensor_calibration_service.srv: -------------------------------------------------------------------------------- 1 | --- 2 | visensor_node/visensor_calibration[] calibration #camera calibration 3 | --------------------------------------------------------------------------------