OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
RadialTangentialDistortion8.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2  * OKVIS - Open Keyframe-based Visual-Inertial SLAM
3  * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
14  * its contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: Jul 28, 2015
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #include <Eigen/LU>
40 #include <iostream>
41 
43 namespace okvis {
45 namespace cameras {
46 
47 // The default constructor with all zero ki
48 RadialTangentialDistortion8::RadialTangentialDistortion8()
49  : k1_(0.0),
50  k2_(0.0),
51  p1_(0.0),
52  p2_(0.0),
53  k3_(0.0),
54  k4_(0.0),
55  k5_(0.0),
56  k6_(0.0)
57 {
58  parameters_.setZero();
59 }
60 
61 // Constructor initialising ki
62 RadialTangentialDistortion8::RadialTangentialDistortion8(double k1, double k2,
63  double p1, double p2,
64  double k3, double k4,
65  double k5, double k6)
66 {
67  parameters_[0] = k1;
68  parameters_[1] = k2;
69  parameters_[2] = p1;
70  parameters_[3] = p2;
71  parameters_[4] = k3;
72  parameters_[5] = k4;
73  parameters_[6] = k5;
74  parameters_[7] = k6;
75  k1_ = k1;
76  k2_ = k2;
77  p1_ = p1;
78  p2_ = p2;
79  k3_ = k3;
80  k4_ = k4;
81  k5_ = k5;
82  k6_ = k6;
83 }
84 
85 bool RadialTangentialDistortion8::setParameters(
86  const Eigen::VectorXd & parameters)
87 {
88  if (parameters.cols() != NumDistortionIntrinsics) {
89  return false;
90  }
91  parameters_ = parameters;
92  k1_ = parameters[0];
93  k2_ = parameters[1];
94  p1_ = parameters[2];
95  p2_ = parameters[3];
96  k3_ = parameters[4];
97  k4_ = parameters[5];
98  k5_ = parameters[6];
99  k6_ = parameters[7];
100  return true;
101 }
102 
103 bool RadialTangentialDistortion8::distort(
104  const Eigen::Vector2d & pointUndistorted,
105  Eigen::Vector2d * pointDistorted) const
106 {
107  // just compute the distorted point
108  const double u0 = pointUndistorted[0];
109  const double u1 = pointUndistorted[1];
110  const double mx_u = u0 * u0;
111  const double my_u = u1 * u1;
112  const double rho_u = mx_u + my_u;
113  if(rho_u>9.0)
114  return false; // to avoid confusion of this model
115  const double mxy_u = u0 * u1;
116  const double rad_dist_u = (1.0 + ((k3_ * rho_u + k2_) * rho_u + k1_) * rho_u)
117  / (1.0 + ((k6_ * rho_u + k5_) * rho_u + k4_) * rho_u);
118  (*pointDistorted)[0] = u0 * rad_dist_u + 2.0 * p1_ * mxy_u
119  + p2_ * (rho_u + 2.0 * mx_u);
120  (*pointDistorted)[1] = u1 * rad_dist_u + 2.0 * p2_ * mxy_u
121  + p1_ * (rho_u + 2.0 * my_u);
122  return true;
123 }
124 
125 bool RadialTangentialDistortion8::distort(
126  const Eigen::Vector2d & pointUndistorted, Eigen::Vector2d * pointDistorted,
127  Eigen::Matrix2d * pointJacobian, Eigen::Matrix2Xd * parameterJacobian) const
128 {
129  // first compute the distorted point
130  const double u0 = pointUndistorted[0];
131  const double u1 = pointUndistorted[1];
132  const double mx_u = u0 * u0;
133  const double my_u = u1 * u1;
134  const double mxy_u = u0 * u1;
135  const double rho_u = mx_u + my_u;
136 
137  if(rho_u>9.0)
138  return false; // to avoid confusion of this model
139 
140  const double c = rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0;
141  const double c2 = c*c;
142 
143  const double rad_dist_u = (1.0 + ((k3_ * rho_u + k2_) * rho_u + k1_) * rho_u)
144  / (1.0 + ((k6_ * rho_u + k5_) * rho_u + k4_) * rho_u);
145  (*pointDistorted)[0] = u0 * rad_dist_u + 2.0 * p1_ * mxy_u
146  + p2_ * (rho_u + 2.0 * mx_u);
147  (*pointDistorted)[1] = u1 * rad_dist_u + 2.0 * p2_ * mxy_u
148  + p1_ * (rho_u + 2.0 * my_u);
149 
150  // next the Jacobian w.r.t. changes on the undistorted point
151  Eigen::Matrix2d & J = *pointJacobian;
152  J(0,0) = p1_*u1*2.0+p2_*u0*6.0+(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0)+(u0*(rho_u*(u0*(k2_+k3_*rho_u)*2.0+k3_*u0*rho_u*2.0)+u0*(k1_+rho_u*(k2_+k3_*rho_u))*2.0))/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0)-u0*(rho_u*(u0*(k5_+k6_*rho_u)*2.0+k6_*u0*rho_u*2.0)+u0*(k4_+rho_u*(k5_+k6_*rho_u))*2.0)*(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)*1.0/c2;
153  J(0,1) = p1_*u0*2.0+p2_*u1*2.0+(u0*(rho_u*(u1*(k2_+k3_*rho_u)*2.0+k3_*u1*rho_u*2.0)+u1*(k1_+rho_u*(k2_+k3_*rho_u))*2.0))/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0)-u0*(rho_u*(u1*(k5_+k6_*rho_u)*2.0+k6_*u1*rho_u*2.0)+u1*(k4_+rho_u*(k5_+k6_*rho_u))*2.0)*(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)*1.0/c2;
154  J(1,0) = p1_*u0*2.0+p2_*u1*2.0+(u1*(rho_u*(u0*(k2_+k3_*rho_u)*2.0+k3_*u0*rho_u*2.0)+u0*(k1_+rho_u*(k2_+k3_*rho_u))*2.0))/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0)-u1*(rho_u*(u0*(k5_+k6_*rho_u)*2.0+k6_*u0*rho_u*2.0)+u0*(k4_+rho_u*(k5_+k6_*rho_u))*2.0)*(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)*1.0/c2;
155  J(1,1) = p1_*u1*6.0+p2_*u0*2.0+(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0)+(u1*(rho_u*(u1*(k2_+k3_*rho_u)*2.0+k3_*u1*rho_u*2.0)+u1*(k1_+rho_u*(k2_+k3_*rho_u))*2.0))/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0)-u1*(rho_u*(u1*(k5_+k6_*rho_u)*2.0+k6_*u1*rho_u*2.0)+u1*(k4_+rho_u*(k5_+k6_*rho_u))*2.0)*(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)*1.0/c2;
156 
157  if (parameterJacobian) {
158  // the Jacobian w.r.t. intrinsics parameters
159  const double rho_u2 = rho_u*rho_u;
160  const double rho_u3 = rho_u*rho_u2;
161  Eigen::Matrix2Xd & Jp = *parameterJacobian;
162  Jp.resize(2, NumDistortionIntrinsics);
163  Jp(0,0) = (u0*rho_u)/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0);
164  Jp(0,1) = (u0*rho_u2)/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0);
165  Jp(0,2) = u0*u1*2.0;
166  Jp(0,3) = (u0*u0)*3.0+u1*u1;
167  Jp(0,4) = (u0*rho_u3)/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0);
168  Jp(0,5) = -u0*rho_u*(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)*1.0/c2;
169  Jp(0,6) = -u0*rho_u2*(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)*1.0/c2;
170  Jp(0,7) = -u0*rho_u3*(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)*1.0/c2;
171  Jp(1,0) = (u1*rho_u)/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0);
172  Jp(1,1) = (u1*rho_u2)/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0);
173  Jp(1,2) = u0*u0+(u1*u1)*3.0;
174  Jp(1,3) = u0*u1*2.0;
175  Jp(1,4) = (u1*rho_u3)/(rho_u*(k4_+rho_u*(k5_+k6_*rho_u))+1.0);
176  Jp(1,5) = -u1*rho_u*(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)*1.0/c2;
177  Jp(1,6) = -u1*rho_u2*(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)*1.0/c2;
178  Jp(1,7) = -u1*rho_u3*(rho_u*(k1_+rho_u*(k2_+k3_*rho_u))+1.0)*1.0/c2;
179  }
180  return true;
181 }
182 
183 bool RadialTangentialDistortion8::distortWithExternalParameters(
184  const Eigen::Vector2d & pointUndistorted,
185  const Eigen::VectorXd & parameters, Eigen::Vector2d * pointDistorted,
186  Eigen::Matrix2d * pointJacobian, Eigen::Matrix2Xd * parameterJacobian) const
187 {
188  const double k1 = parameters[0];
189  const double k2 = parameters[1];
190  const double p1 = parameters[2];
191  const double p2 = parameters[3];
192  const double k3 = parameters[4];
193  const double k4 = parameters[5];
194  const double k5 = parameters[6];
195  const double k6 = parameters[7];
196  // first compute the distorted point
197  const double u0 = pointUndistorted[0];
198  const double u1 = pointUndistorted[1];
199  const double mx_u = u0 * u0;
200  const double my_u = u1 * u1;
201  const double mxy_u = u0 * u1;
202  const double rho_u = mx_u + my_u;
203 
204  if(rho_u>9.0)
205  return false; // to avoid confusion of this model
206 
207  const double c = rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0;
208  const double c2 = c*c;
209 
210  const double rad_dist_u = (1.0 + ((k3 * rho_u + k2) * rho_u + k1) * rho_u)
211  / (1.0 + ((k6 * rho_u + k5) * rho_u + k4) * rho_u);
212  (*pointDistorted)[0] = u0 * rad_dist_u + 2.0 * p1 * mxy_u
213  + p2 * (rho_u + 2.0 * mx_u);
214  (*pointDistorted)[1] = u1 * rad_dist_u + 2.0 * p2 * mxy_u
215  + p1 * (rho_u + 2.0 * my_u);
216 
217  // next the Jacobian w.r.t. changes on the undistorted point
218  Eigen::Matrix2d & J = *pointJacobian;
219  J(0,0) = p1*u1*2.0+p2*u0*6.0+(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0)+(u0*(rho_u*(u0*(k2+k3*rho_u)*2.0+k3*u0*rho_u*2.0)+u0*(k1+rho_u*(k2+k3*rho_u))*2.0))/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0)-u0*(rho_u*(u0*(k5+k6*rho_u)*2.0+k6*u0*rho_u*2.0)+u0*(k4+rho_u*(k5+k6*rho_u))*2.0)*(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)*1.0/c2;
220  J(0,1) = p1*u0*2.0+p2*u1*2.0+(u0*(rho_u*(u1*(k2+k3*rho_u)*2.0+k3*u1*rho_u*2.0)+u1*(k1+rho_u*(k2+k3*rho_u))*2.0))/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0)-u0*(rho_u*(u1*(k5+k6*rho_u)*2.0+k6*u1*rho_u*2.0)+u1*(k4+rho_u*(k5+k6*rho_u))*2.0)*(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)*1.0/c2;
221  J(1,0) = p1*u0*2.0+p2*u1*2.0+(u1*(rho_u*(u0*(k2+k3*rho_u)*2.0+k3*u0*rho_u*2.0)+u0*(k1+rho_u*(k2+k3*rho_u))*2.0))/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0)-u1*(rho_u*(u0*(k5+k6*rho_u)*2.0+k6*u0*rho_u*2.0)+u0*(k4+rho_u*(k5+k6*rho_u))*2.0)*(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)*1.0/c2;
222  J(1,1) = p1*u1*6.0+p2*u0*2.0+(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0)+(u1*(rho_u*(u1*(k2+k3*rho_u)*2.0+k3*u1*rho_u*2.0)+u1*(k1+rho_u*(k2+k3*rho_u))*2.0))/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0)-u1*(rho_u*(u1*(k5+k6*rho_u)*2.0+k6*u1*rho_u*2.0)+u1*(k4+rho_u*(k5+k6*rho_u))*2.0)*(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)*1.0/c2;
223 
224  if (parameterJacobian) {
225  // the Jacobian w.r.t. intrinsics parameters
226  const double rho_u2 = rho_u*rho_u;
227  const double rho_u3 = rho_u*rho_u2;
228  Eigen::Matrix2Xd & Jp = *parameterJacobian;
229  Jp.resize(2, NumDistortionIntrinsics);
230  Jp(0,0) = (u0*rho_u)/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0);
231  Jp(0,1) = (u0*rho_u2)/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0);
232  Jp(0,2) = u0*u1*2.0;
233  Jp(0,3) = (u0*u0)*3.0+u1*u1;
234  Jp(0,4) = (u0*rho_u3)/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0);
235  Jp(0,5) = -u0*rho_u*(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)*1.0/c2;
236  Jp(0,6) = -u0*rho_u2*(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)*1.0/c2;
237  Jp(0,7) = -u0*rho_u3*(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)*1.0/c2;
238  Jp(1,0) = (u1*rho_u)/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0);
239  Jp(1,1) = (u1*rho_u2)/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0);
240  Jp(1,2) = u0*u0+(u1*u1)*3.0;
241  Jp(1,3) = u0*u1*2.0;
242  Jp(1,4) = (u1*rho_u3)/(rho_u*(k4+rho_u*(k5+k6*rho_u))+1.0);
243  Jp(1,5) = -u1*rho_u*(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)*1.0/c2;
244  Jp(1,6) = -u1*rho_u2*(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)*1.0/c2;
245  Jp(1,7) = -u1*rho_u3*(rho_u*(k1+rho_u*(k2+k3*rho_u))+1.0)*1.0/c2;
246  }
247  return true;
248 }
249 bool RadialTangentialDistortion8::undistort(
250  const Eigen::Vector2d & pointDistorted,
251  Eigen::Vector2d * pointUndistorted) const
252 {
253  // this is expensive: we solve with Gauss-Newton...
254  Eigen::Vector2d x_bar = pointDistorted; // initialise at distorted point
255  const int n = 5; // just 5 iterations max.
256  Eigen::Matrix2d E; // error Jacobian
257 
258  bool success = false;
259  for (int i = 0; i < n; i++) {
260 
261  Eigen::Vector2d x_tmp;
262 
263  distort(x_bar, &x_tmp, &E);
264 
265  Eigen::Vector2d e(pointDistorted - x_tmp);
266  Eigen::Matrix2d E2 = (E.transpose() * E);
267  Eigen::Vector2d du = E2.inverse() * E.transpose() * e;
268 
269  x_bar += du;
270 
271  const double chi2 = e.dot(e);
272  if (chi2 < 1e-4) {
273  success = true;
274  }
275  if (chi2 < 1e-15) {
276  success = true;
277  break;
278  }
279 
280  }
281  *pointUndistorted = x_bar;
282 
283  if (!success) {
284  std::cout << (E.transpose() * E) << std::endl;
285  }
286 
287  return success;
288 }
289 bool RadialTangentialDistortion8::undistort(
290  const Eigen::Vector2d & pointDistorted, Eigen::Vector2d * pointUndistorted,
291  Eigen::Matrix2d * pointJacobian) const
292 {
293  // this is expensive: we solve with Gauss-Newton...
294  Eigen::Vector2d x_bar = pointDistorted; // initialise at distorted point
295  const int n = 5; // just 5 iterations max.
296  Eigen::Matrix2d E; // error Jacobian
297 
298  bool success = false;
299  for (int i = 0; i < n; i++) {
300 
301  Eigen::Vector2d x_tmp;
302 
303  distort(x_bar, &x_tmp, &E);
304 
305  Eigen::Vector2d e(pointDistorted - x_tmp);
306  Eigen::Vector2d dx = (E.transpose() * E).inverse() * E.transpose() * e;
307 
308  x_bar += dx;
309 
310  const double chi2 = e.dot(e);
311  if (chi2 < 1e-4) {
312  success = true;
313  }
314  if (chi2 < 1e-15) {
315  success = true;
316  break;
317  }
318 
319  }
320  *pointUndistorted = x_bar;
321 
322  // the Jacobian of the inverse map is simply the inverse Jacobian.
323  *pointJacobian = E.inverse();
324 
325  return success;
326 }
327 
328 } // namespace cameras
329 } // namespace okvis
okvis::cameras::RadialTangentialDistortion8::undistort
bool undistort(const Eigen::Vector2d &pointDistorted, Eigen::Vector2d *pointUndistorted) const
Undistortion only.
Definition: RadialTangentialDistortion8.hpp:249
okvis::cameras::RadialTangentialDistortion8::k1_
double k1_
radial parameter 1
Definition: RadialTangentialDistortion8.hpp:177
okvis::cameras::RadialTangentialDistortion8::k5_
double k5_
radial parameter 3
Definition: RadialTangentialDistortion8.hpp:183
okvis::cameras::RadialTangentialDistortion8::k6_
double k6_
radial parameter 4
Definition: RadialTangentialDistortion8.hpp:184
okvis::cameras::RadialTangentialDistortion8::k2_
double k2_
radial parameter 2
Definition: RadialTangentialDistortion8.hpp:178
okvis::cameras::RadialTangentialDistortion8::distortWithExternalParameters
bool distortWithExternalParameters(const Eigen::Vector2d &pointUndistorted, const Eigen::VectorXd &parameters, Eigen::Vector2d *pointDistorted, Eigen::Matrix2d *pointJacobian=NULL, Eigen::Matrix2Xd *parameterJacobian=NULL) const
Distortion and Jacobians using external distortion intrinsics parameters.
Definition: RadialTangentialDistortion8.hpp:183
okvis::cameras::RadialTangentialDistortion8::RadialTangentialDistortion8
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RadialTangentialDistortion8()
The default constructor with all zero ki.
Definition: RadialTangentialDistortion8.hpp:48
okvis::cameras::RadialTangentialDistortion8::k3_
double k3_
radial parameter 3
Definition: RadialTangentialDistortion8.hpp:181
okvis::cameras::RadialTangentialDistortion8::setParameters
bool setParameters(const Eigen::VectorXd &parameters)
set the generic parameters
Definition: RadialTangentialDistortion8.hpp:85
okvis::cameras::RadialTangentialDistortion8::distort
bool distort(const Eigen::Vector2d &pointUndistorted, Eigen::Vector2d *pointDistorted) const
Distortion only.
Definition: RadialTangentialDistortion8.hpp:103
okvis::cameras::RadialTangentialDistortion8::k4_
double k4_
radial parameter 4
Definition: RadialTangentialDistortion8.hpp:182
okvis::cameras::RadialTangentialDistortion8::p1_
double p1_
tangential parameter 1
Definition: RadialTangentialDistortion8.hpp:179
okvis::cameras::RadialTangentialDistortion8::p2_
double p2_
tangential parameter 2
Definition: RadialTangentialDistortion8.hpp:180
okvis::cameras::RadialTangentialDistortion8::NumDistortionIntrinsics
static const int NumDistortionIntrinsics
Definition: RadialTangentialDistortion8.hpp:100
okvis::cameras::RadialTangentialDistortion8::parameters_
Eigen::Matrix< double, NumDistortionIntrinsics, 1 > parameters_
all distortion parameters
Definition: RadialTangentialDistortion8.hpp:175
Generated on Fri Jul 22 2016 16:09:09 for OKVIS ROS by   doxygen 1.8.6