OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
RadialTangentialDistortion.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
50  : k1_(0.0),
51  k2_(0.0),
52  p1_(0.0),
53  p2_(0.0)
54 {
55  parameters_.setZero();
56 }
57 
58 // Constructor initialising ki
60  double p1, double p2)
61 {
62  parameters_[0] = k1;
63  parameters_[1] = k2;
64  parameters_[2] = p1;
65  parameters_[3] = p2;
66  k1_ = k1;
67  k2_ = k2;
68  p1_ = p1;
69  p2_ = p2;
70 }
71 
73  const Eigen::VectorXd & parameters)
74 {
75  if (parameters.cols() != NumDistortionIntrinsics) {
76  return false;
77  }
78  parameters_ = parameters;
79  k1_ = parameters[0];
80  k2_ = parameters[1];
81  p1_ = parameters[2];
82  p2_ = parameters[3];
83  return true;
84 }
85 
87  const Eigen::Vector2d & pointUndistorted,
88  Eigen::Vector2d * pointDistorted) const
89 {
90  // just compute the distorted point
91  const double u0 = pointUndistorted[0];
92  const double u1 = pointUndistorted[1];
93  const double mx_u = u0 * u0;
94  const double my_u = u1 * u1;
95  const double mxy_u = u0 * u1;
96  const double rho_u = mx_u + my_u;
97  const double rad_dist_u = k1_ * rho_u + k2_ * rho_u * rho_u;
98  (*pointDistorted)[0] = u0 + u0 * rad_dist_u + 2.0 * p1_ * mxy_u
99  + p2_ * (rho_u + 2.0 * mx_u);
100  (*pointDistorted)[1] = u1 + u1 * rad_dist_u + 2.0 * p2_ * mxy_u
101  + p1_ * (rho_u + 2.0 * my_u);
102  return true;
103 
104 }
106  const Eigen::Vector2d & pointUndistorted, Eigen::Vector2d * pointDistorted,
107  Eigen::Matrix2d * pointJacobian,
108  Eigen::Matrix2Xd * parameterJacobian) const
109 {
110  // first compute the distorted point
111  const double u0 = pointUndistorted[0];
112  const double u1 = pointUndistorted[1];
113  const double mx_u = u0 * u0;
114  const double my_u = u1 * u1;
115  const double mxy_u = u0 * u1;
116  const double rho_u = mx_u + my_u;
117  const double rad_dist_u = k1_ * rho_u + k2_ * rho_u * rho_u;
118  (*pointDistorted)[0] = u0 + u0 * rad_dist_u + 2.0 * p1_ * mxy_u
119  + p2_ * (rho_u + 2.0 * mx_u);
120  (*pointDistorted)[1] = u1 + u1 * rad_dist_u + 2.0 * p2_ * mxy_u
121  + p1_ * (rho_u + 2.0 * my_u);
122 
123  // next the Jacobian w.r.t. changes on the undistorted point
124  Eigen::Matrix2d & J = *pointJacobian;
125  J(0, 0) = 1 + rad_dist_u + k1_ * 2.0 * mx_u + k2_ * rho_u * 4 * mx_u
126  + 2.0 * p1_ * u1 + 6 * p2_ * u0;
127  J(1, 0) = k1_ * 2.0 * u0 * u1 + k2_ * 4 * rho_u * u0 * u1 + p1_ * 2.0 * u0
128  + 2.0 * p2_ * u1;
129  J(0, 1) = J(1, 0);
130  J(1, 1) = 1 + rad_dist_u + k1_ * 2.0 * my_u + k2_ * rho_u * 4 * my_u
131  + 6 * p1_ * u1 + 2.0 * p2_ * u0;
132 
133  if (parameterJacobian) {
134  // the Jacobian w.r.t. intrinsics parameters
135  Eigen::Matrix2Xd & J2 = *parameterJacobian;
136  J2.resize(2,NumDistortionIntrinsics);
137  const double r2 = rho_u;
138  const double r4 = r2 * r2;
139 
140  //[ u0*(u0^2 + u1^2), u0*(u0^2 + u1^2)^2, 2*u0*u1, 3*u0^2 + u1^2]
141  //[ u1*(u0^2 + u1^2), u1*(u0^2 + u1^2)^2, u0^2 + 3*u1^2, 2*u0*u1]
142 
143  J2(0, 0) = u0 * r2;
144  J2(0, 1) = u0 * r4;
145  J2(0, 2) = 2.0 * u0 * u1;
146  J2(0, 3) = r2 + 2.0 * u0 * u0;
147 
148  J2(1, 0) = u1 * r2;
149  J2(1, 1) = u1 * r4;
150  J2(1, 2) = r2 + 2.0 * u1 * u1;
151  J2(1, 3) = 2.0 * u0 * u1;
152  }
153  return true;
154 }
156  const Eigen::Vector2d & pointUndistorted,
157  const Eigen::VectorXd & parameters, Eigen::Vector2d * pointDistorted,
158  Eigen::Matrix2d * pointJacobian,
159  Eigen::Matrix2Xd * parameterJacobian) const
160 {
161  const double k1 = parameters[0];
162  const double k2 = parameters[1];
163  const double p1 = parameters[2];
164  const double p2 = parameters[3];
165  // first compute the distorted point
166  const double u0 = pointUndistorted[0];
167  const double u1 = pointUndistorted[1];
168  const double mx_u = u0 * u0;
169  const double my_u = u1 * u1;
170  const double mxy_u = u0 * u1;
171  const double rho_u = mx_u + my_u;
172  const double rad_dist_u = k1 * rho_u + k2 * rho_u * rho_u;
173  (*pointDistorted)[0] = u0 + u0 * rad_dist_u + 2.0 * p1 * mxy_u
174  + p2 * (rho_u + 2.0 * mx_u);
175  (*pointDistorted)[1] = u1 + u1 * rad_dist_u + 2.0 * p2 * mxy_u
176  + p1 * (rho_u + 2.0 * my_u);
177 
178  // next the Jacobian w.r.t. changes on the undistorted point
179  Eigen::Matrix2d & J = *pointJacobian;
180  J(0, 0) = 1 + rad_dist_u + k1 * 2.0 * mx_u + k2 * rho_u * 4 * mx_u
181  + 2.0 * p1 * u1 + 6 * p2 * u0;
182  J(1, 0) = k1 * 2.0 * u0 * u1 + k2 * 4 * rho_u * u0 * u1 + p1 * 2.0 * u0
183  + 2.0 * p2 * u1;
184  J(0, 1) = J(1, 0);
185  J(1, 1) = 1 + rad_dist_u + k1 * 2.0 * my_u + k2 * rho_u * 4 * my_u
186  + 6 * p1 * u1 + 2.0 * p2 * u0;
187 
188  if (parameterJacobian) {
189  // the Jacobian w.r.t. intrinsics parameters
190  Eigen::Matrix2Xd & J2 = *parameterJacobian;
191  J2.resize(2,NumDistortionIntrinsics);
192  const double r2 =rho_u;
193  const double r4 = r2 * r2;
194 
195  //[ u0*(u0^2 + u1^2), u0*(u0^2 + u1^2)^2, 2*u0*u1, 3*u0^2 + u1^2]
196  //[ u1*(u0^2 + u1^2), u1*(u0^2 + u1^2)^2, u0^2 + 3*u1^2, 2*u0*u1]
197 
198  J2(0, 0) = u0 * r2;
199  J2(0, 1) = u0 * r4;
200  J2(0, 2) = 2.0 * u0 * u1;
201  J2(0, 3) = r2 + 2.0 * u0 * u0;
202 
203  J2(1, 0) = u1 * r2;
204  J2(1, 1) = u1 * r4;
205  J2(1, 2) = r2 + 2.0 * u1 * u1;
206  J2(1, 3) = 2.0 * u0 * u1;
207  }
208  return true;
209 }
211  const Eigen::Vector2d & pointDistorted,
212  Eigen::Vector2d * pointUndistorted) const
213 {
214  // this is expensive: we solve with Gauss-Newton...
215  Eigen::Vector2d x_bar = pointDistorted; // initialise at distorted point
216  const int n = 5; // just 5 iterations max.
217  Eigen::Matrix2d E; // error Jacobian
218 
219  bool success = false;
220  for (int i = 0; i < n; i++) {
221 
222  Eigen::Vector2d x_tmp;
223 
224  distort(x_bar, &x_tmp, &E);
225 
226  Eigen::Vector2d e(pointDistorted - x_tmp);
227  Eigen::Matrix2d E2 = (E.transpose() * E);
228  Eigen::Vector2d du = E2.inverse() * E.transpose() * e;
229 
230  x_bar += du;
231 
232  const double chi2 = e.dot(e);
233  if (chi2 < 1e-4) {
234  success = true;
235  }
236  if (chi2 < 1e-15) {
237  success = true;
238  break;
239  }
240 
241  }
242  *pointUndistorted = x_bar;
243 
244  if(!success){
245  std::cout<<(E.transpose() * E)<<std::endl;
246  }
247 
248  return success;
249 }
251  const Eigen::Vector2d & pointDistorted, Eigen::Vector2d * pointUndistorted,
252  Eigen::Matrix2d * pointJacobian) const
253 {
254  // this is expensive: we solve with Gauss-Newton...
255  Eigen::Vector2d x_bar = pointDistorted; // initialise at distorted point
256  const int n = 5; // just 5 iterations max.
257  Eigen::Matrix2d E; // error Jacobian
258 
259  bool success = false;
260  for (int i = 0; i < n; i++) {
261 
262  Eigen::Vector2d x_tmp;
263 
264  distort(x_bar, &x_tmp, &E);
265 
266  Eigen::Vector2d e(pointDistorted - x_tmp);
267  Eigen::Vector2d dx = (E.transpose() * E).inverse() * E.transpose() * e;
268 
269  x_bar += dx;
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  // the Jacobian of the inverse map is simply the inverse Jacobian.
284  *pointJacobian = E.inverse();
285 
286  return success;
287 }
288 
289 } // namespace cameras
290 } // namespace okvis
Eigen::Matrix< double, NumDistortionIntrinsics, 1 > parameters_
all distortion parameters
Definition: RadialTangentialDistortion.hpp:167
double k1_
radial parameter 1
Definition: RadialTangentialDistortion.hpp:169
bool distort(const Eigen::Vector2d &pointUndistorted, Eigen::Vector2d *pointDistorted) const
Distortion only.
Definition: RadialTangentialDistortion.hpp:86
double p1_
tangential parameter 1
Definition: RadialTangentialDistortion.hpp:171
double p2_
tangential parameter 2
Definition: RadialTangentialDistortion.hpp:172
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: RadialTangentialDistortion.hpp:155
bool setParameters(const Eigen::VectorXd &parameters)
set the generic parameters
Definition: RadialTangentialDistortion.hpp:72
bool undistort(const Eigen::Vector2d &pointDistorted, Eigen::Vector2d *pointUndistorted) const
Undistortion only.
Definition: RadialTangentialDistortion.hpp:210
double k2_
radial parameter 2
Definition: RadialTangentialDistortion.hpp:170
static const int NumDistortionIntrinsics
Definition: RadialTangentialDistortion.hpp:94
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RadialTangentialDistortion()
The default constructor with all zero ki.
Definition: RadialTangentialDistortion.hpp:49