├── README.md
├── vi_sensor_interface
    ├── CMakeLists.txt
    ├── README.md
    ├── cmake
    │   ├── FindEigen3.cmake
    │   └── FindVISensor.cmake
    ├── include
    │   ├── ConcurrentQueue.hpp
    │   └── vi_sensor_interface.hpp
    └── src
    │   ├── main.cpp
    │   └── vi_sensor_interface.cpp
└── vi_sensor_stereo_block_matcher
    ├── CMakeLists.txt
    ├── README.md
    ├── cmake
        ├── FindEigen3.cmake
        └── FindVISensor.cmake
    ├── include
        └── vi_sensor_interface.hpp
    └── src
        ├── main.cpp
        └── vi_sensor_interface.cpp


/README.md:
--------------------------------------------------------------------------------
1 | visensor_sample_applications
2 | ===================
3 | 
4 | This repo contains a few sample standalone applications for the VI-Sensor
5 | 
6 | * **vi_sensor_interface**: A very basic interface to the VI-Sensor. Receives images and IMU messages and displays it using openCV.
7 | * **vi_sensor_stereo_block_matcher**: Applies rectification to Vi-Sensor images and subsequently computes the disparity image using standard openCV block matcher. Intrinsic & extrinsic calibration parameters for rectification are downloaded from VI-Sensor.
8 | 


--------------------------------------------------------------------------------
/vi_sensor_interface/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | #Sammy Omari, Skybotix AG, 28/3/2014
 2 | cmake_minimum_required(VERSION 2.8.0)
 3 |  
 4 | ############
 5 | # SETTINGS
 6 | ############
 7 | set(PKGNAME vi_sensor_interface)
 8 | 
 9 | set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH})
10 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
11 | set(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
12 | set(Boost_USE_MULTITHREADED ON)
13 | 
14 | find_package(Boost COMPONENTS system filesystem thread REQUIRED)
15 | find_package(Threads REQUIRED)
16 | find_package(Eigen3 REQUIRED)
17 | find_package(OpenCV REQUIRED )
18 | find_package(VISensor REQUIRED )
19 | 
20 | set(CMAKE_BUILD_TYPE Release)
21 | ADD_DEFINITIONS (-march=native -O2 -fmessage-length=0 -MMD -MP -Wall -pedantic -std=c++0x ) #Release
22 | #ADD_DEFINITIONS (-march=native -O0 -g3 -fmessage-length=0 -MMD -MP -Wall -pedantic -std=c++0x ) #Debug
23 | 
24 | FILE(
25 |   GLOB SRCS 
26 |   src/*.cpp
27 | )
28 | 
29 | include_directories( ${EIGEN3_INCLUDE_DIR} )
30 | include_directories( ${Boost_INCLUDE_DIRS} )  
31 | include_directories( ${VISensorDriver_INCLUDE_DIR} )  
32 | include_directories(include)
33 | 
34 | add_executable(${PKGNAME} ${SRCS})
35 | target_link_libraries(${PKGNAME} ${OpenCV_LIBS} ${VISensorDriver_LIBRARY} visensor  ${Boost_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
36 | 


--------------------------------------------------------------------------------
/vi_sensor_interface/README.md:
--------------------------------------------------------------------------------
 1 | vi_sensor_interface
 2 | ===================
 3 | 
 4 | Simple example on how to interface the VI-Sensor and display the images using openCV
 5 | 
 6 | Build instructions
 7 | ==================
 8 | Download libvisensor and install it if you have not done that before
 9 | 
10 | <pre><code>
11 | git clone https://github.com/ethz-asl/libvisensor.git
12 | cd libvisensor
13 | ./install_libvisensor.sh
14 | </code></pre>
15 | 
16 | Now, were building the interface
17 | 
18 | <pre><code>
19 | cd vi_sensor_interface
20 | mkdir build
21 | cd build
22 | cmake ..
23 | make
24 | cd ../bin
25 | ./vi_sensor_interface
26 | </code></pre>
27 | 
28 | 


--------------------------------------------------------------------------------
/vi_sensor_interface/cmake/FindEigen3.cmake:
--------------------------------------------------------------------------------
 1 | # - Try to find Eigen3 lib
 2 | # Once done this will define
 3 | #
 4 | #  EIGEN3_FOUND - system has eigen lib
 5 | #  EIGEN3_INCLUDE_DIR - the eigen include directory
 6 | 
 7 | # Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
 8 | # Redistribution and use is allowed according to the terms of the BSD license.
 9 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file.
10 | 
11 | if( EIGEN3_INCLUDE_DIR )
12 |     # in cache already
13 |     set( EIGEN3_FOUND TRUE )
14 | else (EIGEN3_INCLUDE_DIR)
15 |     find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core
16 |         PATH_SUFFIXES eigen3/
17 |         HINTS
18 |         ${INCLUDE_INSTALL_DIR}
19 |         /usr/local/include
20 |         ${KDE4_INCLUDE_DIR}
21 |         )
22 |     include( FindPackageHandleStandardArgs )
23 |     find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR )   
24 |     mark_as_advanced( EIGEN3_INCLUDE_DIR )
25 | endif(EIGEN3_INCLUDE_DIR)
26 | 
27 | 


--------------------------------------------------------------------------------
/vi_sensor_interface/cmake/FindVISensor.cmake:
--------------------------------------------------------------------------------
 1 | find_package(PkgConfig)
 2 | pkg_check_modules(PC_VISENSOR visensor)
 3 | message("pkg: ${PC_VISENSOR_INCLUDEDIRS}")
 4 | set(VISENSOR_DEFINITIONS ${PC_VISENSOR_CFLAGS_OTHER})
 5 | 
 6 | find_path(VISensorDriver_INCLUDE_DIR visensor.hpp
 7 |           HINTS ${PC_VISENSOR_INCLUDEDIR} ${PC_VISENSOR_INCLUDE_DIRS}
 8 |           PATH_SUFFIXES visensor )
 9 | 
10 | find_library(VISensorDriver_LIBRARY NAMES libvisensor.so
11 |              HINTS ${PC_VISENSOR_LIBDIR} ${PC_VISENSOR_LIBRARY_DIRS} /usr/local/lib)
12 | 
13 | set(VISENSOR_LIBRARIES ${VISENSOR_LIBRARY} )
14 | set(VISENSOR_INCLUDE_DIRS ${VISENSOR_INCLUDE_DIR} )
15 | 
16 | include(FindPackageHandleStandardArgs)
17 | # handle the QUIETLY and REQUIRED arguments and set LIBXML2_FOUND to TRUE
18 | # if all listed variables are TRUE
19 | find_package_handle_standard_args(VISensorDriver  DEFAULT_MSG
20 |                                   VISensorDriver_LIBRARY VISensorDriver_INCLUDE_DIR)
21 | 
22 | mark_as_advanced(VISensorDriver_INCLUDE_DIR VISensorDriver_LIBRARY )
23 | 


--------------------------------------------------------------------------------
/vi_sensor_interface/include/ConcurrentQueue.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com)
  3 |  *
  4 |  * All rights reserved.
  5 |  *
  6 |  * Redistribution and non-commercial use in source and binary forms, with or without
  7 |  * modification, are permitted provided that the following conditions are met:
  8 |  *
  9 |  * Redistributions of source code must retain the above copyright notice, this
 10 |  * list of conditions and the following disclaimer.
 11 |  *
 12 |  * Redistributions in binary form must reproduce the above copyright notice, this
 13 |  * list of conditions and the following disclaimer in the documentation and/or
 14 |  * other materials provided with the distribution.
 15 |  *
 16 |  * Neither the name of the {organization} nor the names of its
 17 |  * contributors may be used to endorse or promote products derived from
 18 |  * this software without specific prior written permission.
 19 |  *
 20 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 21 |  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 22 |  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 23 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 24 |  * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 25 |  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 26 |  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 27 |  * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 28 |  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 29 |  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 30 |  *
 31 |  */
 32 | 
 33 | #ifndef CONCURRENTQUEUE_HPP_
 34 | #define CONCURRENTQUEUE_HPP_
 35 | 
 36 | #include <queue>
 37 | 
 38 | #include <boost/thread.hpp>
 39 | #include <boost/foreach.hpp>
 40 | #include <boost/thread/mutex.hpp>
 41 | #include <boost/thread/condition_variable.hpp>
 42 | 
 43 | template<typename T>
 44 | class ConcurrentQueue {
 45 |  private:
 46 |   std::queue<T> q_;
 47 |   boost::mutex m_;
 48 |   boost::condition_variable c_;
 49 |   unsigned int cntr;
 50 | 
 51 |  public:
 52 |   ConcurrentQueue() {
 53 |     cntr = 0;
 54 |   };
 55 | 
 56 |   void push(const T& data) {
 57 |     boost::lock_guard<boost::mutex> l(m_);
 58 |     q_.push(data);
 59 |     c_.notify_one();
 60 |   }
 61 | 
 62 |   //if the queue is empty and we pop, we just wait for data!!
 63 |   T pop() {
 64 |     boost::mutex::scoped_lock l(m_);
 65 | 
 66 |     if (q_.size() == 0) {
 67 |       c_.wait(l);
 68 |     }
 69 | 
 70 |     T res = q_.front();
 71 |     q_.pop();
 72 |     return res;
 73 |   }
 74 | 
 75 |   //checks queue is empty
 76 |   bool empty() {
 77 |     boost::mutex::scoped_lock l(m_);
 78 |     return q_.size() == 0;
 79 |   }
 80 | 
 81 |   //return size of queue
 82 |   size_t size() {
 83 |     boost::mutex::scoped_lock l(m_);
 84 |     return q_.size();
 85 |   }
 86 | 
 87 |   void clear() {
 88 |     std::queue<int> empty;
 89 |     std::swap(q_, empty);
 90 |   }
 91 | 
 92 |   //returns current queue and clear it
 93 |   std::queue<T> pop_all() {
 94 |     boost::mutex::scoped_lock l(m_);
 95 | 
 96 |     std::queue<T> queue; //empty here
 97 |     std::swap(queue, q_);
 98 | 
 99 |     return queue;
100 |   }
101 | 
102 |   //returns current queue and clear it
103 |   std::queue<T> clone() {
104 |     boost::mutex::scoped_lock l(m_);
105 | 
106 |     std::queue<T> queue = q_;
107 |     return queue;
108 |   }
109 | 
110 | };
111 | 
112 | #endif /* CONCURRENTQUEUE_HPP_ */
113 | 


--------------------------------------------------------------------------------
/vi_sensor_interface/include/vi_sensor_interface.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com)
 3 |  *
 4 |  * All rights reserved.
 5 |  *
 6 |  * Redistribution and non-commercial use in source and binary forms, with or without
 7 |  * modification, are permitted provided that the following conditions are met:
 8 |  *
 9 |  * Redistributions of source code must retain the above copyright notice, this
10 |  * list of conditions and the following disclaimer.
11 |  *
12 |  * Redistributions in binary form must reproduce the above copyright notice, this
13 |  * list of conditions and the following disclaimer in the documentation and/or
14 |  * other materials provided with the distribution.
15 |  *
16 |  * Neither the name of the {organization} nor the names of its
17 |  * contributors may be used to endorse or promote products derived from
18 |  * this software without specific prior written permission.
19 |  *
20 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
21 |  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
22 |  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
24 |  * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25 |  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26 |  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
27 |  * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
28 |  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29 |  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 |  *
31 |  */
32 | 
33 | #include <vector>
34 | #include <iostream>
35 | #include <ctime>
36 | #include <fstream>      
37 | #include <chrono>
38 | 
39 | #include <boost/bind.hpp>
40 | #include <boost/smart_ptr.hpp>
41 | #include <boost/shared_ptr.hpp>
42 | #include <boost/thread/mutex.hpp>
43 | #include <boost/filesystem.hpp>
44 | 
45 | #include <Eigen/Core>
46 | #include <Eigen/Dense>
47 | 
48 | #include <opencv2/opencv.hpp>
49 | 
50 | #include <visensor/visensor.hpp>
51 | #include "ConcurrentQueue.hpp"
52 | 
53 | class ViSensorInterface {
54 |  public:
55 |   ViSensorInterface();
56 |   ViSensorInterface(uint32_t image_rate, uint32_t imu_rate);
57 |   ~ViSensorInterface();
58 | 
59 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60 |  private:
61 |   visensor::ViSensorDriver drv_;
62 |   typedef ConcurrentQueue<visensor::ViFrame::Ptr> ViFrameQueue;
63 | 
64 |   ViFrameQueue frameQueue[4];
65 | 
66 |   bool vi_sensor_connected_;
67 |   boost::mutex io_mutex_;
68 | 
69 |   void StartIntegratedSensor(uint32_t image_rate, uint32_t imu_rate);
70 |   void ImageCallback(visensor::ViFrame::Ptr frame_ptr);
71 |   void ImuCallback(boost::shared_ptr<visensor::ViImuMsg> imu_ptr);
72 |   void worker(unsigned int cam_id);
73 | };
74 | 


--------------------------------------------------------------------------------
/vi_sensor_interface/src/main.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com)
 3 |  *
 4 |  * All rights reserved.
 5 |  *
 6 |  * Redistribution and non-commercial use in source and binary forms, with or without
 7 |  * modification, are permitted provided that the following conditions are met:
 8 |  *
 9 |  * Redistributions of source code must retain the above copyright notice, this
10 |  * list of conditions and the following disclaimer.
11 |  *
12 |  * Redistributions in binary form must reproduce the above copyright notice, this
13 |  * list of conditions and the following disclaimer in the documentation and/or
14 |  * other materials provided with the distribution.
15 |  *
16 |  * Neither the name of the {organization} nor the names of its
17 |  * contributors may be used to endorse or promote products derived from
18 |  * this software without specific prior written permission.
19 |  *
20 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
21 |  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
22 |  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
24 |  * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25 |  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26 |  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
27 |  * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
28 |  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29 |  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 |  *
31 |  */
32 | 
33 | #include <vi_sensor_interface.hpp>
34 | 
35 | int main(int argc, char** argv) {
36 | 
37 |   ViSensorInterface viSensor;
38 | 
39 |   return 0;
40 | }
41 | 


--------------------------------------------------------------------------------
/vi_sensor_interface/src/vi_sensor_interface.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com)
  3 |  *
  4 |  * All rights reserved.
  5 |  *
  6 |  * Redistribution and non-commercial use in source and binary forms, with or without
  7 |  * modification, are permitted provided that the following conditions are met:
  8 |  *
  9 |  * Redistributions of source code must retain the above copyright notice, this
 10 |  * list of conditions and the following disclaimer.
 11 |  *
 12 |  * Redistributions in binary form must reproduce the above copyright notice, this
 13 |  * list of conditions and the following disclaimer in the documentation and/or
 14 |  * other materials provided with the distribution.
 15 |  *
 16 |  * Neither the name of the {organization} nor the names of its
 17 |  * contributors may be used to endorse or promote products derived from
 18 |  * this software without specific prior written permission.
 19 |  *
 20 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 21 |  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 22 |  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 23 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 24 |  * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 25 |  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 26 |  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 27 |  * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 28 |  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 29 |  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 30 |  *
 31 |  */
 32 | #include <vi_sensor_interface.hpp>
 33 | 
 34 | ViSensorInterface::ViSensorInterface(uint32_t image_rate, uint32_t imu_rate)
 35 |     : vi_sensor_connected_(false) {
 36 |   StartIntegratedSensor(image_rate, imu_rate);
 37 | }
 38 | 
 39 | ViSensorInterface::ViSensorInterface()
 40 |     : vi_sensor_connected_(false) {
 41 |   StartIntegratedSensor(20, 200);
 42 | }
 43 | 
 44 | ViSensorInterface::~ViSensorInterface() {
 45 | 
 46 | }
 47 | 
 48 | void ViSensorInterface::StartIntegratedSensor(uint32_t image_rate, uint32_t imu_rate) {
 49 | 
 50 |   if (image_rate > 30) {
 51 |     image_rate = 30;
 52 |     std::cout << "Desired image rate is too hight, setting it to 30 Hz." << std::endl;
 53 |   }
 54 |   if (imu_rate > 800) {
 55 |     imu_rate = 800;
 56 |     std::cout << "Desired imu rate is too hight, setting it to 800 Hz." << std::endl;
 57 |   }
 58 | 
 59 |   try {
 60 |     drv_.init();
 61 |   } catch (visensor::exceptions::ConnectionException const &ex) {
 62 |     std::cout << ex.what() << "\n";
 63 |     return;
 64 |   }
 65 | 
 66 |   std::cout << "Libvisensor version is " << visensor::LIBRARY_VERSION_MAJOR << "."
 67 |       << visensor::LIBRARY_VERSION_MINOR << "." << visensor::LIBRARY_VERSION_PATCH << std::endl;
 68 | 
 69 |   // load available calibration and configure sensors corresponding to it
 70 |   drv_.selectCameraCalibration();
 71 | 
 72 |   // set callback for image messages
 73 |   drv_.setCameraCallback(boost::bind(&ViSensorInterface::ImageCallback, this, _1));
 74 | 
 75 |   // set callback for IMU messages
 76 |   drv_.setImuCallback(boost::bind(&ViSensorInterface::ImuCallback, this, _1));
 77 | 
 78 |   drv_.startAllCameras(image_rate);
 79 |   drv_.startAllImus(imu_rate);
 80 |   vi_sensor_connected_ = true;
 81 | 
 82 |   boost::thread drawWorker_0(boost::bind(&ViSensorInterface::worker, this, 0));
 83 |   boost::thread drawWorker_1(boost::bind(&ViSensorInterface::worker, this, 1));
 84 |   boost::thread drawWorker_2(boost::bind(&ViSensorInterface::worker, this, 2));
 85 |   boost::thread drawWorker_3(boost::bind(&ViSensorInterface::worker, this, 3));
 86 | 
 87 |   drawWorker_0.join();
 88 |   drawWorker_1.join();
 89 |   drawWorker_2.join();
 90 |   drawWorker_3.join();
 91 | }
 92 | 
 93 | void ViSensorInterface::ImageCallback(visensor::ViFrame::Ptr frame_ptr) {
 94 |   uint32_t camera_id = frame_ptr->camera_id;
 95 | 	//push image on queue
 96 |   frameQueue[camera_id].push(frame_ptr);
 97 | }
 98 | 
 99 | void ViSensorInterface::ImuCallback(boost::shared_ptr<visensor::ViImuMsg> imu_ptr) {
100 |   if (imu_ptr->imu_id != visensor::SensorId::IMU0)
101 |     return;
102 |   uint32_t timeNSec = imu_ptr->timestamp;
103 |   Eigen::Vector3d gyro(imu_ptr->gyro[0], imu_ptr->gyro[1], imu_ptr->gyro[2]);
104 |   Eigen::Vector3d acc(imu_ptr->acc[0], imu_ptr->acc[1], imu_ptr->acc[2]);
105 |   std::cout << "IMU Accelerometer data: " << acc[0] << "\t "<< acc[1] << "\t"<< acc[2] << "\t" << std::endl;
106 | 	//Do stuff with IMU...
107 | }
108 | 
109 | void ViSensorInterface::worker(unsigned int cam_id) {
110 |   while (1) {
111 | 		//Popping image from queue. If no image available, we perform a blocking wait
112 |     visensor::ViFrame::Ptr frame = frameQueue[cam_id].pop();
113 |     uint32_t camera_id = frame->camera_id;
114 |     cv::Mat image;
115 |     image.create(frame->height, frame->width, CV_8UC1);
116 |     memcpy(image.data, frame->getImageRawPtr(), frame->height * frame->width);
117 |     //update window with image
118 |     char winName[255];
119 |     boost::mutex::scoped_lock lock(io_mutex_);  //lock thread as opencv does seem to have problems with multithreading
120 |     sprintf(winName, "Camera %u", camera_id);
121 |     cv::namedWindow(winName, CV_WINDOW_AUTOSIZE);
122 |     cv::imshow(winName, image);
123 |     cv::waitKey(1);
124 |   }
125 | }
126 | 


--------------------------------------------------------------------------------
/vi_sensor_stereo_block_matcher/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | #Sammy Omari, Skybotix AG, 28/3/2014
 2 | cmake_minimum_required(VERSION 2.8.0)
 3 |  
 4 | ############
 5 | # SETTINGS
 6 | ############
 7 | set(PKGNAME vi_sensor_stereo_block_matcher)
 8 | 
 9 | set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH})
10 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
11 | set(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
12 | set(Boost_USE_MULTITHREADED ON)
13 | 
14 | find_package(Boost COMPONENTS system filesystem thread REQUIRED)
15 | find_package(Threads REQUIRED)
16 | find_package(Eigen3 REQUIRED)
17 | find_package( OpenCV REQUIRED )
18 | find_package(VISensor REQUIRED )
19 | 
20 | set(CMAKE_BUILD_TYPE Release)
21 | ADD_DEFINITIONS (-march=native -O2 -fmessage-length=0 -MMD -MP -Wall -pedantic -std=c++0x ) #Release
22 | #ADD_DEFINITIONS (-march=native -O0 -g3 -fmessage-length=0 -MMD -MP -Wall -pedantic -std=c++0x ) #Debug
23 | 
24 | FILE(
25 |   GLOB SRCS 
26 |   src/*.cpp
27 | )
28 | 
29 | include_directories( ${EIGEN3_INCLUDE_DIR} )
30 | include_directories( ${Boost_INCLUDE_DIRS} )  
31 | include_directories( ${VISensorDriver_INCLUDE_DIR} )  
32 | include_directories(include)
33 | 
34 | add_executable(${PKGNAME} ${SRCS})
35 | target_link_libraries(${PKGNAME} ${OpenCV_LIBS} ${VISensorDriver_LIBRARY} visensor  ${Boost_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
36 | 


--------------------------------------------------------------------------------
/vi_sensor_stereo_block_matcher/README.md:
--------------------------------------------------------------------------------
 1 | vi_sensor_stereo_block_matcher
 2 | ===================
 3 | 
 4 | Simple example on how to compute a stereo disparity image using the VI-Sensor
 5 | 
 6 | Build instructions
 7 | ==================
 8 | Download libvisensor and install it if you have not done that before
 9 | 
10 | <pre><code>
11 | git clone https://github.com/ethz-asl/libvisensor.git
12 | cd libvisensor
13 | ./install_libvisensor.sh
14 | </code></pre>
15 | 
16 | Now, were building the stereo block matcher
17 | 
18 | <pre><code>
19 | cd vi_sensor_stereo_block_matcher
20 | mkdir build
21 | cd build
22 | cmake ..
23 | make
24 | cd ../bin
25 | ./vi_sensor_stereo_block_matcher
26 | </code></pre>
27 | 


--------------------------------------------------------------------------------
/vi_sensor_stereo_block_matcher/cmake/FindEigen3.cmake:
--------------------------------------------------------------------------------
 1 | # - Try to find Eigen3 lib
 2 | # Once done this will define
 3 | #
 4 | #  EIGEN3_FOUND - system has eigen lib
 5 | #  EIGEN3_INCLUDE_DIR - the eigen include directory
 6 | 
 7 | # Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
 8 | # Redistribution and use is allowed according to the terms of the BSD license.
 9 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file.
10 | 
11 | if( EIGEN3_INCLUDE_DIR )
12 |     # in cache already
13 |     set( EIGEN3_FOUND TRUE )
14 | else (EIGEN3_INCLUDE_DIR)
15 |     find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core
16 |         PATH_SUFFIXES eigen3/
17 |         HINTS
18 |         ${INCLUDE_INSTALL_DIR}
19 |         /usr/local/include
20 |         ${KDE4_INCLUDE_DIR}
21 |         )
22 |     include( FindPackageHandleStandardArgs )
23 |     find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR )   
24 |     mark_as_advanced( EIGEN3_INCLUDE_DIR )
25 | endif(EIGEN3_INCLUDE_DIR)
26 | 
27 | 


--------------------------------------------------------------------------------
/vi_sensor_stereo_block_matcher/cmake/FindVISensor.cmake:
--------------------------------------------------------------------------------
 1 | find_package(PkgConfig)
 2 | pkg_check_modules(PC_VISENSOR visensor)
 3 | message("pkg: ${PC_VISENSOR_INCLUDEDIRS}")
 4 | set(VISENSOR_DEFINITIONS ${PC_VISENSOR_CFLAGS_OTHER})
 5 | 
 6 | find_path(VISensorDriver_INCLUDE_DIR visensor.hpp
 7 |           HINTS ${PC_VISENSOR_INCLUDEDIR} ${PC_VISENSOR_INCLUDE_DIRS}
 8 |           PATH_SUFFIXES visensor )
 9 | 
10 | find_library(VISensorDriver_LIBRARY NAMES libvisensor.so
11 |              HINTS ${PC_VISENSOR_LIBDIR} ${PC_VISENSOR_LIBRARY_DIRS} /usr/local/lib)
12 | 
13 | set(VISENSOR_LIBRARIES ${VISENSOR_LIBRARY} )
14 | set(VISENSOR_INCLUDE_DIRS ${VISENSOR_INCLUDE_DIR} )
15 | 
16 | include(FindPackageHandleStandardArgs)
17 | # handle the QUIETLY and REQUIRED arguments and set LIBXML2_FOUND to TRUE
18 | # if all listed variables are TRUE
19 | find_package_handle_standard_args(VISensorDriver  DEFAULT_MSG
20 |                                   VISensorDriver_LIBRARY VISensorDriver_INCLUDE_DIR)
21 | 
22 | mark_as_advanced(VISensorDriver_INCLUDE_DIR VISensorDriver_LIBRARY )
23 | 


--------------------------------------------------------------------------------
/vi_sensor_stereo_block_matcher/include/vi_sensor_interface.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com)
 3 |  *
 4 |  * All rights reserved.
 5 |  *
 6 |  * Redistribution and non-commercial use in source and binary forms, with or without
 7 |  * modification, are permitted provided that the following conditions are met:
 8 |  *
 9 |  * Redistributions of source code must retain the above copyright notice, this
10 |  * list of conditions and the following disclaimer.
11 |  *
12 |  * Redistributions in binary form must reproduce the above copyright notice, this
13 |  * list of conditions and the following disclaimer in the documentation and/or
14 |  * other materials provided with the distribution.
15 |  *
16 |  * Neither the name of the {organization} nor the names of its
17 |  * contributors may be used to endorse or promote products derived from
18 |  * this software without specific prior written permission.
19 |  *
20 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
21 |  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
22 |  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
24 |  * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25 |  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26 |  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
27 |  * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
28 |  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29 |  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 |  *
31 |  */
32 | 
33 | #include <vector>
34 | #include <iostream>
35 | #include <ctime>
36 | #include <fstream>      
37 | #include <chrono>
38 | 
39 | #include <boost/bind.hpp>
40 | #include <boost/smart_ptr.hpp>
41 | #include <boost/shared_ptr.hpp>
42 | #include <boost/thread/mutex.hpp>
43 | #include <boost/thread/condition_variable.hpp>
44 | #include <boost/filesystem.hpp>
45 | 
46 | #include <Eigen/Core>
47 | #include <Eigen/Dense>
48 | 
49 | #include <opencv2/opencv.hpp>
50 | #include <opencv2/imgproc/imgproc.hpp>
51 | #include <opencv2/highgui/highgui.hpp>
52 | #include <opencv2/calib3d/calib3d.hpp>
53 | 
54 | #include <visensor/visensor.hpp>
55 | 
56 | class ViSensorInterface {
57 |  public:
58 |   ViSensorInterface();
59 |   ViSensorInterface(uint32_t image_rate, uint32_t imu_rate);
60 |   ~ViSensorInterface();
61 |   void run(void);
62 | 
63 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64 |  private:
65 |   void StartIntegratedSensor(uint32_t image_rate, uint32_t imu_rate);
66 |   void ImageCallback(visensor::ViFrame::Ptr frame_ptr);
67 |   void ImuCallback(boost::shared_ptr<visensor::ViImuMsg> imu_ptr);
68 |   void process_data();
69 |   bool computeRectificationMaps(void);
70 | 
71 |   visensor::ViSensorDriver drv_;
72 |   typedef std::deque<visensor::ViFrame::Ptr> ViFrameQueue;
73 |   visensor::SensorId::SensorId idxCam0_;
74 |   visensor::SensorId::SensorId idxCam1_;
75 | 
76 |   ViFrameQueue frameQueue[4];
77 | 
78 |   boost::mutex io_mutex_;
79 |   bool computed_rectification_map_;
80 |   cv::Mat map00_, map01_, map10_, map11_;
81 |   cv::StereoBM bm_;
82 | };
83 | 


--------------------------------------------------------------------------------
/vi_sensor_stereo_block_matcher/src/main.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com)
 3 |  *
 4 |  * All rights reserved.
 5 |  *
 6 |  * Redistribution and non-commercial use in source and binary forms, with or without
 7 |  * modification, are permitted provided that the following conditions are met:
 8 |  *
 9 |  * Redistributions of source code must retain the above copyright notice, this
10 |  * list of conditions and the following disclaimer.
11 |  *
12 |  * Redistributions in binary form must reproduce the above copyright notice, this
13 |  * list of conditions and the following disclaimer in the documentation and/or
14 |  * other materials provided with the distribution.
15 |  *
16 |  * Neither the name of the {organization} nor the names of its
17 |  * contributors may be used to endorse or promote products derived from
18 |  * this software without specific prior written permission.
19 |  *
20 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
21 |  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
22 |  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
24 |  * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25 |  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26 |  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
27 |  * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
28 |  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29 |  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 |  *
31 |  */
32 | #include <vi_sensor_interface.hpp>
33 | 
34 | int main(int argc, char** argv) {
35 | 
36 |   ViSensorInterface visensor;
37 |   visensor.run();
38 |   return 0;
39 | }
40 | 


--------------------------------------------------------------------------------
/vi_sensor_stereo_block_matcher/src/vi_sensor_interface.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright (c) 2014, Skybotix AG, Switzerland (info@skybotix.com)
  3 |  *
  4 |  * All rights reserved.
  5 |  *
  6 |  * Redistribution and non-commercial use in source and binary forms, with or without
  7 |  * modification, are permitted provided that the following conditions are met:
  8 |  *
  9 |  * Redistributions of source code must retain the above copyright notice, this
 10 |  * list of conditions and the following disclaimer.
 11 |  *
 12 |  * Redistributions in binary form must reproduce the above copyright notice, this
 13 |  * list of conditions and the following disclaimer in the documentation and/or
 14 |  * other materials provided with the distribution.
 15 |  *
 16 |  * Neither the name of the {organization} nor the names of its
 17 |  * contributors may be used to endorse or promote products derived from
 18 |  * this software without specific prior written permission.
 19 |  *
 20 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 21 |  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 22 |  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 23 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 24 |  * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 25 |  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 26 |  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 27 |  * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 28 |  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 29 |  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 30 |  *
 31 |  */
 32 | #include <vi_sensor_interface.hpp>
 33 | 
 34 | ViSensorInterface::ViSensorInterface(uint32_t image_rate, uint32_t imu_rate)
 35 |     : computed_rectification_map_(false) {
 36 |   StartIntegratedSensor(image_rate, imu_rate);
 37 | }
 38 | 
 39 | ViSensorInterface::ViSensorInterface()
 40 |     : computed_rectification_map_(false) {
 41 | 
 42 |   StartIntegratedSensor(20, 200);
 43 | }
 44 | 
 45 | ViSensorInterface::~ViSensorInterface() {
 46 | 
 47 | }
 48 | 
 49 | void ViSensorInterface::run(void) {
 50 |   // set callback for completed frames
 51 |   drv_.setCameraCallback(boost::bind(&ViSensorInterface::ImageCallback, this, _1));
 52 | 
 53 |   // set callback for completed IMU messages
 54 |   drv_.setImuCallback(boost::bind(&ViSensorInterface::ImuCallback, this, _1));
 55 | 
 56 |   //Lets wait for the user to end program.
 57 |   //This is ok as program is callback-driven anyways
 58 |   boost::mutex m;
 59 |   boost::mutex::scoped_lock l(m);
 60 |   boost::condition_variable c;
 61 |   c.wait(l);
 62 | }
 63 | 
 64 | void ViSensorInterface::StartIntegratedSensor(uint32_t image_rate, uint32_t imu_rate) {
 65 | 
 66 |   if (image_rate > 30) {
 67 |     image_rate = 30;
 68 |     std::cout << "Desired image rate is too high, setting it to 30 Hz." << std::endl;
 69 |   }
 70 |   if (imu_rate > 800) {
 71 |     imu_rate = 800;
 72 |     std::cout << "Desired imu rate is too high, setting it to 800 Hz." << std::endl;
 73 |   }
 74 | 
 75 |   try {
 76 |     drv_.init();
 77 |   } catch (visensor::exceptions::ConnectionException const &ex) {
 78 |     std::cout << ex.what() << "\n";
 79 |     return;
 80 |   }
 81 | 
 82 |   std::cout << "Libvisensor version is " << visensor::LIBRARY_VERSION_MAJOR << "."
 83 |       << visensor::LIBRARY_VERSION_MINOR << "." << visensor::LIBRARY_VERSION_PATCH << std::endl;
 84 | 
 85 |   // load available calibration and configure sensors corresponding to it
 86 |   drv_.selectCameraCalibration();
 87 | 
 88 |   //determine which camera is left and which is right (required for opencv block matcher)
 89 |   if (!drv_.isStereoCameraFlipped()){
 90 |     idxCam0_ = visensor::SensorId::CAM0;
 91 |     idxCam1_ = visensor::SensorId::CAM1;
 92 |   }else{
 93 |     idxCam0_ = visensor::SensorId::CAM1;
 94 |     idxCam1_ = visensor::SensorId::CAM0;
 95 |   }
 96 | 
 97 |   drv_.startAllCameras(image_rate);
 98 |   drv_.startAllImus(imu_rate);
 99 | }
100 | 
101 | void ViSensorInterface::ImageCallback(visensor::ViFrame::Ptr frame_ptr) {
102 |   boost::mutex::scoped_lock lock(io_mutex_);  //lock thread as opencv does seem to have problems with multithreading
103 |   uint32_t camera_id = frame_ptr->camera_id;
104 |   frameQueue[camera_id].emplace_back(frame_ptr);
105 |   process_data();
106 | }
107 | 
108 | void ViSensorInterface::ImuCallback(boost::shared_ptr<visensor::ViImuMsg> imu_ptr) {
109 |   Eigen::Vector3d gyro(imu_ptr->gyro[0], imu_ptr->gyro[1], imu_ptr->gyro[2]);
110 |   Eigen::Vector3d acc(imu_ptr->acc[0], imu_ptr->acc[1], imu_ptr->acc[2]);
111 | }
112 | 
113 | void ViSensorInterface::process_data() {
114 | 
115 |   //Lets sync image streams from cam0 & cam1
116 |   if (frameQueue[idxCam0_].empty() || frameQueue[idxCam1_].empty())
117 |     return;
118 |   while (frameQueue[idxCam0_].front()->timestamp > frameQueue[idxCam1_].front()->timestamp) {
119 |     frameQueue[idxCam1_].pop_front();
120 |     if (frameQueue[idxCam0_].empty() || frameQueue[idxCam1_].empty())
121 |       return;
122 |   }
123 |   while (frameQueue[idxCam0_].front()->timestamp < frameQueue[idxCam1_].front()->timestamp) {
124 |     frameQueue[idxCam0_].pop_front();
125 |     if (frameQueue[idxCam0_].empty() || frameQueue[idxCam1_].empty())
126 |       return;
127 |   }
128 | 
129 |   //If we arrive here, both queues have synced timestamps
130 |   visensor::ViFrame::Ptr frame0 = frameQueue[idxCam0_].front();
131 |   visensor::ViFrame::Ptr frame1 = frameQueue[idxCam1_].front();
132 | 
133 |   if (!computed_rectification_map_)
134 |     computeRectificationMaps();
135 | 
136 |   cv::Mat img0, img1;
137 |   img0.create(frame0->height, frame0->width, CV_8UC1);
138 |   img0.data = frame0->getImageRawPtr();
139 | 
140 |   img1.create(frame1->height, frame1->width, CV_8UC1);
141 |   img1.data = frame1->getImageRawPtr();
142 | 
143 |   cv::Mat img0rect, img1rect;
144 |   cv::remap(img0, img0rect, map00_, map01_, cv::INTER_LINEAR);
145 |   cv::remap(img1, img1rect, map10_, map11_, cv::INTER_LINEAR);
146 | 
147 |   cv::namedWindow("img_rect_cam0", CV_WINDOW_AUTOSIZE);
148 |   cv::imshow("img_rect_cam0", img0rect);
149 | 
150 |   cv::Mat disp, disp8;
151 |   bm_(img0rect, img1rect, disp);
152 |   disp.convertTo(disp8, CV_8U);
153 |   cv::namedWindow("disparity", CV_WINDOW_AUTOSIZE);
154 |   cv::imshow("disparity", disp8);
155 |   cv::waitKey(1);
156 | 
157 |   //We dont need the frames anymore, lets drop them
158 |   frameQueue[idxCam0_].pop_front();
159 |   frameQueue[idxCam1_].pop_front();
160 | }
161 | 
162 | bool ViSensorInterface::computeRectificationMaps(void) {
163 |   visensor::ViCameraCalibration camera_calibration_0, camera_calibration_1;
164 |   try {
165 |     visensor::ViCameraCalibration tmp;
166 |     std::vector<visensor::ViCameraCalibration> calibrations;
167 |     drv_.getSelectedCameraCalibration(&tmp, idxCam0_);
168 |     if (tmp.lens_model_->type_ != visensor::ViCameraLensModel::LensModelTypes::RADTAN ||
169 |         tmp.projection_model_->type_ != visensor::ViCameraProjectionModel::ProjectionModelTypes::PINHOLE) {
170 |       std::cerr << "No radtan and pinhole calibration specified. Therefore the factory "
171 |           "calibration is choosen for the ROS sensor message." << std::endl;
172 |       calibrations = drv_.getCameraCalibrations(
173 |           idxCam0_, 0, tmp.is_flipped_,
174 |           visensor::ViCameraLensModel::LensModelTypes::RADTAN,
175 |           visensor::ViCameraProjectionModel::ProjectionModelTypes::PINHOLE);
176 |       if (calibrations.size() == 0) {
177 |         std::cerr << "No corresponding Factory Calibration for cam left found" << std::endl;
178 |         return false;
179 |       }
180 |       camera_calibration_0 = calibrations.front();
181 |     }
182 |     else {
183 |       camera_calibration_0 = tmp;
184 |     }
185 |     drv_.getSelectedCameraCalibration(&tmp, idxCam1_);
186 |     if (tmp.lens_model_->type_ != visensor::ViCameraLensModel::LensModelTypes::RADTAN ||
187 |         tmp.projection_model_->type_ != visensor::ViCameraProjectionModel::ProjectionModelTypes::PINHOLE) {
188 |       std::cerr << "No radtan and pinhole calibration specified. "
189 |           "Therefore the factory calibration is choosen for the ROS sensor message." << std::endl;
190 |       calibrations = drv_.getCameraCalibrations(
191 |           idxCam1_, 0, tmp.is_flipped_,
192 |           visensor::ViCameraLensModel::LensModelTypes::RADTAN,
193 |           visensor::ViCameraProjectionModel::ProjectionModelTypes::PINHOLE);
194 |       if (calibrations.size() == 0) {
195 |         std::cerr << "No corresponding Factory Calibration for cam right found" << std::endl;
196 |         return false;
197 |       }
198 |       camera_calibration_1 = calibrations.front();
199 |     }
200 |     else {
201 |       camera_calibration_1 = tmp;
202 |     }
203 |   }
204 |   catch (visensor::exceptions const &ex) {
205 |     std::cerr << "Could not read the camera configuration of cameras "<< idxCam0_ << " and "
206 |         << idxCam1_ << ". Exception: " <<  ex.what() << std::endl;
207 |     return false;
208 |   }
209 | 
210 |   int image_width = camera_calibration_0.resolution_[0];
211 |   int image_height = camera_calibration_0.resolution_[1];;
212 | 
213 |   double c0[9];
214 |   double d0[5];
215 |   double r0[9];
216 |   double p0[12];
217 |   double rot0[9];
218 |   double t0[3];
219 | 
220 |   double c1[9];
221 |   double d1[5];
222 |   double r1[9];
223 |   double p1[12];
224 |   double rot1[9];
225 |   double t1[3];
226 | 
227 |   double r[9];
228 |   double t[3];
229 | 
230 |   std::vector<double> lens_coefficients = camera_calibration_0.lens_model_->getCoefficients();
231 |   std::fill(d0, d0 + 5, 0);
232 |   for ( unsigned int i = 0; i < lens_coefficients.size(); ++i ) {
233 |     d0[i] = lens_coefficients.at(i);
234 |   }
235 |   visensor::ViCameraProjectionModelPinhole::Ptr cam0_projection_model =
236 |       camera_calibration_0.getProjectionModel<visensor::ViCameraProjectionModelPinhole>();
237 |   c0[0] = cam0_projection_model->focal_length_u_;
238 |   c0[1] = 0.0;
239 |   c0[2] = cam0_projection_model->principal_point_u_;
240 |   c0[3] = 0.0;
241 |   c0[4] = cam0_projection_model->focal_length_v_;
242 |   c0[5] = cam0_projection_model->principal_point_v_;
243 |   c0[6] = 0.0;
244 |   c0[7] = 0.0;
245 |   c0[8] = 1.0;
246 | 
247 |   lens_coefficients = camera_calibration_1.lens_model_->getCoefficients();
248 |   std::fill(d1, d1 + 5, 0);
249 |   for ( unsigned int i = 0; i < lens_coefficients.size(); ++i ) {
250 |     d1[i] = lens_coefficients.at(i);
251 |   }
252 |   visensor::ViCameraProjectionModelPinhole::Ptr cam1_projection_model =
253 |       camera_calibration_1.getProjectionModel<visensor::ViCameraProjectionModelPinhole>();
254 |   c1[0] = cam1_projection_model->focal_length_u_;
255 |   c1[1] = 0.0;
256 |   c1[2] = cam1_projection_model->principal_point_u_;
257 |   c1[3] = 0.0;
258 |   c1[4] = cam1_projection_model->focal_length_v_;
259 |   c1[5] = cam1_projection_model->principal_point_v_;
260 |   c1[6] = 0.0;
261 |   c1[7] = 0.0;
262 |   c1[8] = 1.0;
263 | 
264 |   for (int i = 0; i < 9; ++i) {
265 |     rot0[i] = camera_calibration_0.R_[i];
266 |     rot1[i] = camera_calibration_1.R_[i];
267 |   }
268 |   for (int i = 0; i < 3; ++i) {
269 |     t0[i] = camera_calibration_0.t_[i];
270 |     t1[i] = camera_calibration_1.t_[i];
271 |   }
272 | 
273 |   Eigen::Map < Eigen::Matrix3d > RR0(rot0);
274 |   Eigen::Map < Eigen::Vector3d > tt0(t0);
275 |   Eigen::Map < Eigen::Matrix3d > RR1(rot1);
276 |   Eigen::Map < Eigen::Vector3d > tt1(t1);
277 | 
278 |   Eigen::Matrix4d T0 = Eigen::Matrix4d::Zero();
279 |   Eigen::Matrix4d T1 = Eigen::Matrix4d::Zero();
280 | 
281 |   T0.block<3, 3>(0, 0) = RR0;
282 |   T0.block<3, 1>(0, 3) = tt0;
283 |   T0(3, 3) = 1.0;
284 |   T1.block<3, 3>(0, 0) = RR1;
285 |   T1.block<3, 1>(0, 3) = tt1;
286 |   T1(3, 3) = 1.0;
287 | 
288 |   Eigen::Matrix4d T_rel = Eigen::Matrix4d::Zero();
289 |   T_rel = T1 * T0.inverse();
290 | 
291 |   Eigen::Map < Eigen::Matrix3d > R_rel(r);
292 |   Eigen::Map < Eigen::Vector3d > t_rel(t);
293 | 
294 |   R_rel = T_rel.block<3, 3>(0, 0);
295 |   t_rel << T_rel(0, 3), T_rel(1, 3), T_rel(2, 3);
296 | 
297 |   double r_temp[9];
298 |   r_temp[0] = R_rel(0, 0);
299 |   r_temp[1] = R_rel(0, 1);
300 |   r_temp[2] = R_rel(0, 2);
301 |   r_temp[3] = R_rel(1, 0);
302 |   r_temp[4] = R_rel(1, 1);
303 |   r_temp[5] = R_rel(1, 2);
304 |   r_temp[6] = R_rel(2, 0);
305 |   r_temp[7] = R_rel(2, 1);
306 |   r_temp[8] = R_rel(2, 2);
307 | 
308 |   //cv::Mat wrapped(rows, cols, CV_32FC1, external_mem, CV_AUTOSTEP);
309 |   cv::Mat C0(3, 3, CV_64FC1, c0, 3 * sizeof(double));
310 |   cv::Mat D0(5, 1, CV_64FC1, d0, 1 * sizeof(double));
311 |   cv::Mat R0(3, 3, CV_64FC1, r0, 3 * sizeof(double));
312 |   cv::Mat P0(3, 4, CV_64FC1, p0, 4 * sizeof(double));
313 | 
314 |   cv::Mat C1(3, 3, CV_64FC1, c1, 3 * sizeof(double));
315 |   cv::Mat D1(5, 1, CV_64FC1, d1, 1 * sizeof(double));
316 |   cv::Mat R1(3, 3, CV_64FC1, r1, 3 * sizeof(double));
317 |   cv::Mat P1(3, 4, CV_64FC1, p1, 4 * sizeof(double));
318 | 
319 |   cv::Mat R(3, 3, CV_64FC1, r_temp, 3 * sizeof(double));
320 | 
321 |   cv::Mat T(3, 1, CV_64FC1, t, 1 * sizeof(double));
322 | 
323 |   cv::Size img_size(image_width, image_height);
324 | 
325 |   cv::Rect roi1, roi2;
326 |   cv::Mat Q;
327 | 
328 |   cv::stereoRectify(C0, D0, C1, D1, img_size, R, T, R0, R1, P0, P1, Q, cv::CALIB_ZERO_DISPARITY, 0,
329 |                     img_size, &roi1, &roi2);
330 | 
331 |   cv::initUndistortRectifyMap(C0, D0, R0, P0, img_size, CV_16SC2, map00_, map01_);
332 |   cv::initUndistortRectifyMap(C1, D1, R1, P1, img_size, CV_16SC2, map10_, map11_);
333 | 
334 |   computed_rectification_map_ = true;
335 |   return true;
336 | }
337 | 


--------------------------------------------------------------------------------