OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
EquidistantDistortion.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: Feb 3, 2015
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
40 #include <Eigen/LU>
41 #include <iostream>
42 
44 namespace okvis {
46 namespace cameras {
47 
48 // The default constructor with all zero ki
49 EquidistantDistortion::EquidistantDistortion()
50  : k1_(0.0),
51  k2_(0.0),
52  k3_(0.0),
53  k4_(0.0)
54 {
55  parameters_.setZero();
56 }
57 
58 // Constructor initialising ki
59 EquidistantDistortion::EquidistantDistortion(double k1, double k2, double k3,
60  double k4)
61 {
62  parameters_[0] = k1;
63  parameters_[1] = k2;
64  parameters_[2] = k3;
65  parameters_[3] = k4;
66  k1_ = k1;
67  k2_ = k2;
68  k3_ = k3;
69  k4_ = k4;
70 }
71 
72 bool EquidistantDistortion::setParameters(const Eigen::VectorXd & parameters)
73 {
74  if (parameters.cols() != NumDistortionIntrinsics) {
75  return false;
76  }
77  parameters_ = parameters;
78  k1_ = parameters[0];
79  k2_ = parameters[1];
80  k3_ = parameters[2];
81  k4_ = parameters[3];
82  return true;
83 }
84 
85 bool EquidistantDistortion::distort(const Eigen::Vector2d & pointUndistorted,
86  Eigen::Vector2d * pointDistorted) const
87 {
88  // distortion only:
89  const double u0 = pointUndistorted[0];
90  const double u1 = pointUndistorted[1];
91  const double r = sqrt(u0 * u0 + u1 * u1);
92  const double theta = atan(r);
93  const double theta2 = theta * theta;
94  const double theta4 = theta2 * theta2;
95  const double theta6 = theta4 * theta2;
96  const double theta8 = theta4 * theta4;
97  const double thetad = theta
98  * (1 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8);
99 
100  const double scaling = (r > 1e-8) ? thetad / r : 1.0;
101  (*pointDistorted)[0] = scaling * u0;
102  (*pointDistorted)[1] = scaling * u1;
103  return true;
104 }
105 bool EquidistantDistortion::distort(const Eigen::Vector2d & pointUndistorted,
106  Eigen::Vector2d * pointDistorted,
107  Eigen::Matrix2d * pointJacobian,
108  Eigen::Matrix2Xd * parameterJacobian) const
109 {
110  // distortion first:
111  const double u0 = pointUndistorted[0];
112  const double u1 = pointUndistorted[1];
113  const double r = sqrt(u0 * u0 + u1 * u1);
114  const double theta = atan(r);
115  const double theta2 = theta * theta;
116  const double theta4 = theta2 * theta2;
117  const double theta6 = theta4 * theta2;
118  const double theta8 = theta4 * theta4;
119  const double thetad = theta
120  * (1 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8);
121 
122  const double scaling = (r > 1e-8) ? thetad / r : 1.0;
123  (*pointDistorted)[0] = scaling * u0;
124  (*pointDistorted)[1] = scaling * u1;
125 
126  Eigen::Matrix2d & J = *pointJacobian;
127  if (r > 1e-8) {
128  // mostly matlab generated...
129  double t2;
130  double t3;
131  double t4;
132  double t6;
133  double t7;
134  double t8;
135  double t9;
136  double t11;
137  double t17;
138  double t18;
139  double t19;
140  double t20;
141  double t25;
142 
143  t2 = u0 * u0;
144  t3 = u1 * u1;
145  t4 = t2 + t3;
146  t6 = atan(sqrt(t4));
147  t7 = t6 * t6;
148  t8 = 1.0 / sqrt(t4);
149  t9 = t7 * t7;
150  t11 = 1.0 / ((t2 + t3) + 1.0);
151  t17 = (((k1_ * t7 + k2_ * t9) + k3_ * t7 * t9) + k4_ * (t9 * t9)) + 1.0;
152  t18 = 1.0 / t4;
153  t19 = 1.0 / sqrt(t4 * t4 * t4);
154  t20 = t6 * t8 * t17;
155  t25 = ((k2_ * t6 * t7 * t8 * t11 * u1 * 4.0
156  + k3_ * t6 * t8 * t9 * t11 * u1 * 6.0)
157  + k4_ * t6 * t7 * t8 * t9 * t11 * u1 * 8.0)
158  + k1_ * t6 * t8 * t11 * u1 * 2.0;
159  t4 = ((k2_ * t6 * t7 * t8 * t11 * u0 * 4.0
160  + k3_ * t6 * t8 * t9 * t11 * u0 * 6.0)
161  + k4_ * t6 * t7 * t8 * t9 * t11 * u0 * 8.0)
162  + k1_ * t6 * t8 * t11 * u0 * 2.0;
163  t7 = t11 * t17 * t18 * u0 * u1;
164  J(0, 1) = (t7 + t6 * t8 * t25 * u0) - t6 * t17 * t19 * u0 * u1;
165  J(1, 1) = ((t20 - t3 * t6 * t17 * t19) + t3 * t11 * t17 * t18)
166  + t6 * t8 * t25 * u1;
167  J(0, 0) = ((t20 - t2 * t6 * t17 * t19) + t2 * t11 * t17 * t18)
168  + t6 * t8 * t4 * u0;
169  J(1, 0) = (t7 + t6 * t8 * t4 * u1) - t6 * t17 * t19 * u0 * u1;
170 
171  if (parameterJacobian) {
172  Eigen::Matrix2Xd & Ji = *parameterJacobian;
173  Ji.resize(2,NumDistortionIntrinsics);
174  // mostly matlab generated...
175  double t6;
176  double t2;
177  double t3;
178  double t8;
179  double t10;
180 
181  t6 = u0 * u0 + u1 * u1;
182  t2 = atan(sqrt(t6));
183  t3 = t2 * t2;
184  t8 = t3 * t3;
185  t6 = 1.0 / sqrt(t6);
186  t10 = t8 * t8;
187  Ji(0, 0) = t2 * t3 * t6 * u0;
188  Ji(1, 0) = t2 * t3 * t6 * u1;
189  Ji(0, 1) = t2 * t8 * t6 * u0;
190  Ji(1, 1) = t2 * t8 * t6 * u1;
191  Ji(0, 2) = t2 * t3 * t8 * t6 * u0;
192  Ji(1, 2) = t2 * t3 * t8 * t6 * u1;
193  Ji(0, 3) = t2 * t6 * t10 * u0;
194  Ji(1, 3) = t2 * t6 * t10 * u1;
195 
196  }
197  } else {
198  // handle limit case for [u0,u1]->0
199  if (parameterJacobian) {
200  parameterJacobian->resize(2,NumDistortionIntrinsics);
201  parameterJacobian->setZero();
202  }
203  J.setIdentity();
204  }
205 
206  return true;
207 }
208 bool EquidistantDistortion::distortWithExternalParameters(
209  const Eigen::Vector2d & pointUndistorted,
210  const Eigen::VectorXd & parameters, Eigen::Vector2d * pointDistorted,
211  Eigen::Matrix2d * pointJacobian, Eigen::Matrix2Xd * parameterJacobian) const
212 {
213  // decompose parameters
214 
215  const double k1 = parameters[0];
216  const double k2 = parameters[1];
217  const double k3 = parameters[2];
218  const double k4 = parameters[3];
219  // distortion first:
220  const double u0 = pointUndistorted[0];
221  const double u1 = pointUndistorted[1];
222  const double r = sqrt(u0 * u0 + u1 * u1);
223  const double theta = atan(r);
224  const double theta2 = theta * theta;
225  const double theta4 = theta2 * theta2;
226  const double theta6 = theta4 * theta2;
227  const double theta8 = theta4 * theta4;
228  const double thetad = theta
229  * (1 + k1 * theta2 + k2 * theta4 + k3 * theta6 + k4 * theta8);
230 
231  const double scaling = (r > 1e-8) ? thetad / r : 1.0;
232  (*pointDistorted)[0] = scaling * u0;
233  (*pointDistorted)[1] = scaling * u1;
234 
235  Eigen::Matrix2d & J = *pointJacobian;
236  if (r > 1e-8) {
237  // mostly matlab generated...
238  double t2;
239  double t3;
240  double t4;
241  double t6;
242  double t7;
243  double t8;
244  double t9;
245  double t11;
246  double t17;
247  double t18;
248  double t19;
249  double t20;
250  double t25;
251 
252  t2 = u0 * u0;
253  t3 = u1 * u1;
254  t4 = t2 + t3;
255  t6 = atan(sqrt(t4));
256  t7 = t6 * t6;
257  t8 = 1.0 / sqrt(t4);
258  t9 = t7 * t7;
259  t11 = 1.0 / ((t2 + t3) + 1.0);
260  t17 = (((k1 * t7 + k2 * t9) + k3 * t7 * t9) + k4 * (t9 * t9)) + 1.0;
261  t18 = 1.0 / t4;
262  t19 = 1.0 / sqrt(t4 * t4 * t4);
263  t20 = t6 * t8 * t17;
264  t25 = ((k2 * t6 * t7 * t8 * t11 * u1 * 4.0
265  + k3 * t6 * t8 * t9 * t11 * u1 * 6.0)
266  + k4 * t6 * t7 * t8 * t9 * t11 * u1 * 8.0)
267  + k1 * t6 * t8 * t11 * u1 * 2.0;
268  t4 = ((k2 * t6 * t7 * t8 * t11 * u0 * 4.0
269  + k3 * t6 * t8 * t9 * t11 * u0 * 6.0)
270  + k4 * t6 * t7 * t8 * t9 * t11 * u0 * 8.0)
271  + k1 * t6 * t8 * t11 * u0 * 2.0;
272  t7 = t11 * t17 * t18 * u0 * u1;
273  J(0, 0) = (t7 + t6 * t8 * t25 * u0) - t6 * t17 * t19 * u0 * u1;
274  J(1, 0) = ((t20 - t3 * t6 * t17 * t19) + t3 * t11 * t17 * t18)
275  + t6 * t8 * t25 * u1;
276  J(0, 1) = ((t20 - t2 * t6 * t17 * t19) + t2 * t11 * t17 * t18)
277  + t6 * t8 * t4 * u0;
278  J(1, 1) = (t7 + t6 * t8 * t4 * u1) - t6 * t17 * t19 * u0 * u1;
279  if (parameterJacobian) {
280  Eigen::Matrix2Xd & Ji = *parameterJacobian;
281  Ji.resize(2,NumDistortionIntrinsics);
282  // mostly matlab generated...
283  double t6;
284  double t2;
285  double t3;
286  double t8;
287  double t10;
288 
289  t6 = u0 * u0 + u1 * u1;
290  t2 = atan(sqrt(t6));
291  t3 = t2 * t2;
292  t8 = t3 * t3;
293  t6 = 1.0 / sqrt(t6);
294  t10 = t8 * t8;
295  Ji(0, 0) = t2 * t3 * t6 * u0;
296  Ji(1, 0) = t2 * t3 * t6 * u1;
297  Ji(0, 1) = t2 * t8 * t6 * u0;
298  Ji(1, 1) = t2 * t8 * t6 * u1;
299  Ji(0, 2) = t2 * t3 * t8 * t6 * u0;
300  Ji(1, 2) = t2 * t3 * t8 * t6 * u1;
301  Ji(0, 3) = t2 * t6 * t10 * u0;
302  Ji(1, 3) = t2 * t6 * t10 * u1;
303 
304  }
305  } else {
306  // handle limit case for [u0,u1]->0
307  if (parameterJacobian) {
308  parameterJacobian->resize(2,NumDistortionIntrinsics);
309  parameterJacobian->setZero();
310  }
311  J.setIdentity();
312  }
313 
314  return true;
315 }
316 bool EquidistantDistortion::undistort(const Eigen::Vector2d & pointDistorted,
317  Eigen::Vector2d * pointUndistorted) const
318 {
319  // this is expensive: we solve with Gauss-Newton...
320  Eigen::Vector2d x_bar = pointDistorted; // initialise at distorted point
321  const int n = 5; // just 5 iterations max.
322  Eigen::Matrix2d E; // error Jacobian
323 
324  bool success = false;
325  for (int i = 0; i < n; i++) {
326 
327  Eigen::Vector2d x_tmp;
328 
329  distort(x_bar, &x_tmp, &E);
330 
331  Eigen::Vector2d e(pointDistorted - x_tmp);
332  Eigen::Vector2d du = (E.transpose() * E).inverse() * E.transpose() * e;
333 
334  x_bar += du;
335 
336  const double chi2 = e.dot(e);
337  if (chi2 < 1e-2) {
338  success = true;
339  }
340  //std::cout<<"chi2"<<chi2<<std::endl;
341  if (chi2 < 1e-15) {
342  success = true;
343  break;
344  }
345 
346  }
347  *pointUndistorted = x_bar;
348 
349  return success;
350 }
351 bool EquidistantDistortion::undistort(const Eigen::Vector2d & pointDistorted,
352  Eigen::Vector2d * pointUndistorted,
353  Eigen::Matrix2d * pointJacobian) const
354 {
355  // this is expensive: we solve with Gauss-Newton...
356  Eigen::Vector2d x_bar = pointDistorted; // initialise at distorted point
357  const int n = 5; // just 5 iterations max.
358  Eigen::Matrix2d E; // error Jacobian
359 
360  bool success = false;
361  for (int i = 0; i < n; i++) {
362 
363  Eigen::Vector2d x_tmp;
364 
365  distort(x_bar, &x_tmp, &E);
366 
367  Eigen::Vector2d e(pointDistorted - x_tmp);
368  Eigen::Vector2d dx = (E.transpose() * E).inverse() * E.transpose() * e;
369 
370  x_bar += dx;
371 
372  const double chi2 = e.dot(e);
373  if (chi2 < 1e-2) {
374  success = true;
375  }
376  if (chi2 < 1e-15) {
377  success = true;
378  break;
379  }
380 
381  }
382  *pointUndistorted = x_bar;
383 
384  // the Jacobian of the inverse map is simply the inverse Jacobian.
385  *pointJacobian = E.inverse();
386 
387  return success;
388 }
389 
390 } // namespace cameras
391 } // namespace okvis
okvis::cameras::EquidistantDistortion::parameters_
Eigen::Matrix< double, NumDistortionIntrinsics, 1 > parameters_
all distortion parameters
Definition: EquidistantDistortion.hpp:167
okvis::cameras::EquidistantDistortion::EquidistantDistortion
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EquidistantDistortion()
The default constructor with all zero ki.
Definition: EquidistantDistortion.hpp:49
okvis::cameras::EquidistantDistortion::k2_
double k2_
radial parameter 2
Definition: EquidistantDistortion.hpp:170
okvis::cameras::EquidistantDistortion::setParameters
bool setParameters(const Eigen::VectorXd &parameters)
set the generic parameters
Definition: EquidistantDistortion.hpp:72
okvis::cameras::EquidistantDistortion::k1_
double k1_
radial parameter 1
Definition: EquidistantDistortion.hpp:169
okvis::cameras::EquidistantDistortion::NumDistortionIntrinsics
static const int NumDistortionIntrinsics
Definition: EquidistantDistortion.hpp:94
okvis::cameras::EquidistantDistortion::distort
bool distort(const Eigen::Vector2d &pointUndistorted, Eigen::Vector2d *pointDistorted) const
Distortion only.
Definition: EquidistantDistortion.hpp:85
okvis::cameras::EquidistantDistortion::k4_
double k4_
tangential parameter 2
Definition: EquidistantDistortion.hpp:172
okvis::cameras::EquidistantDistortion::k3_
double k3_
tangential parameter 1
Definition: EquidistantDistortion.hpp:171
okvis::cameras::EquidistantDistortion::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: EquidistantDistortion.hpp:208
okvis::cameras::EquidistantDistortion::undistort
bool undistort(const Eigen::Vector2d &pointDistorted, Eigen::Vector2d *pointUndistorted) const
Undistortion only.
Definition: EquidistantDistortion.hpp:316
Generated on Fri Jul 22 2016 16:09:09 for OKVIS ROS by   doxygen 1.8.6