g2o
isometry3d_gradients.h
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #ifndef G2O_ISOMETRY3D_GRADIENTS_H_
28 #define G2O_ISOMETRY3D_GRADIENTS_H_
29 
30 #include "g2o_types_slam3d_api.h"
31 #include "isometry3d_mappings.h"
32 #include "dquat2mat.h"
33 
34 #include <Eigen/Core>
35 #include <Eigen/Geometry>
36 
37 namespace g2o {
38  namespace internal {
39  // forward declaration
40  /* void G2O_TYPES_SLAM3D_API compute_dq_dR (Eigen::Matrix<double, 3 , 9, Eigen::ColMajor>& dq_dR , const double& r11 , const double& r21 , const double& r31 , const double& r12 , const double& r22 , const double& r32 , const double& r13 , const double& r23 , const double& r33 ); */
41 
42  template <typename Derived, typename DerivedOther>
43  inline void skew(Eigen::MatrixBase<Derived>& s, const Eigen::MatrixBase<DerivedOther>& v){
44  const double x=2*v(0);
45  const double y=2*v(1);
46  const double z=2*v(2);
47  s << 0., z, -y, -z, 0, x, y, -x, 0;
48  }
49 
50  template <typename Derived, typename DerivedOther>
51  inline void skewT(Eigen::MatrixBase<Derived>& s, const Eigen::MatrixBase<DerivedOther>& v){
52  const double x=2*v(0);
53  const double y=2*v(1);
54  const double z=2*v(2);
55  s << 0., -z, y, z, 0, -x, -y, x, 0;
56  }
57 
58  template <typename Derived, typename DerivedOther>
59  void skew(Eigen::MatrixBase<Derived>& Sx,
60  Eigen::MatrixBase<Derived>& Sy,
61  Eigen::MatrixBase<Derived>& Sz,
62  const Eigen::MatrixBase<DerivedOther>& R){
63  const double
64  r11=2*R(0,0), r12=2*R(0,1), r13=2*R(0,2),
65  r21=2*R(1,0), r22=2*R(1,1), r23=2*R(1,2),
66  r31=2*R(2,0), r32=2*R(2,1), r33=2*R(2,2);
67  Sx << 0, 0, 0, -r31, -r32, -r33, r21, r22, r23;
68  Sy << r31, r32, r33, 0, 0, 0, -r11, -r12, -r13;
69  Sz << -r21, -r22, -r23, r11, r12, r13, 0, 0, 0;
70  }
71 
72  template <typename Derived, typename DerivedOther>
73  inline void skewT(Eigen::MatrixBase<Derived>& Sx,
74  Eigen::MatrixBase<Derived>& Sy,
75  Eigen::MatrixBase<Derived>& Sz,
76  const Eigen::MatrixBase<DerivedOther>& R){
77  const double
78  r11=2*R(0,0), r12=2*R(0,1), r13=2*R(0,2),
79  r21=2*R(1,0), r22=2*R(1,1), r23=2*R(1,2),
80  r31=2*R(2,0), r32=2*R(2,1), r33=2*R(2,2);
81  Sx << 0, 0, 0, r31, r32, r33, -r21, -r22, -r23;
82  Sy << -r31, -r32, -r33, 0, 0, 0, r11, r12, r13;
83  Sz << r21, r22, r23, -r11, -r12, -r13, 0, 0, 0;
84  }
85 
86  template <typename Derived>
87  void computeEdgeSE3Gradient(Isometry3D& E,
88  Eigen::MatrixBase<Derived> const & JiConstRef,
89  Eigen::MatrixBase<Derived> const & JjConstRef,
90  const Isometry3D& Z,
91  const Isometry3D& Xi,
92  const Isometry3D& Xj,
93  const Isometry3D& Pi/*=Isometry3D()*/,
94  const Isometry3D& Pj/*=Isometry3D()*/)
95  {
96  Eigen::MatrixBase<Derived>& Ji = const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
97  Eigen::MatrixBase<Derived>& Jj = const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
98  Ji.derived().resize(6,6);
99  Jj.derived().resize(6,6);
100  // compute the error at the linearization point
101  const Isometry3D A=Z.inverse()*Pi.inverse();
102  const Isometry3D B=Xi.inverse()*Xj;
103  const Isometry3D& C=Pj;
104 
105  const Isometry3D AB=A*B;
106  const Isometry3D BC=B*C;
107  E=AB*C;
108 
109  Isometry3D::ConstLinearPart Re = extractRotation(E);
110  Isometry3D::ConstLinearPart Ra = extractRotation(A);
111  //const Matrix3D Rb = extractRotation(B);
112  Isometry3D::ConstLinearPart Rc = extractRotation(C);
113  Isometry3D::ConstTranslationPart tc = C.translation();
114  //Isometry3D::ConstTranslationParttab=AB.translation();
115  Isometry3D::ConstLinearPart Rab = extractRotation(AB);
116  Isometry3D::ConstTranslationPart tbc = BC.translation();
117  Isometry3D::ConstLinearPart Rbc = extractRotation(BC);
118 
119  Eigen::Matrix<double, 3 , 9, Eigen::ColMajor> dq_dR;
120  compute_dq_dR (dq_dR,
121  Re(0,0),Re(1,0),Re(2,0),
122  Re(0,1),Re(1,1),Re(2,1),
123  Re(0,2),Re(1,2),Re(2,2));
124 
125  Ji.setZero();
126  Jj.setZero();
127 
128  // dte/dti
129  Ji.template block<3,3>(0,0)=-Ra;
130 
131  // dte/dtj
132  Jj.template block<3,3>(0,0)=Rab;
133 
134  // dte/dqi
135  {
136  Matrix3D S;
137  skewT(S,tbc);
138  Ji.template block<3,3>(0,3)=Ra*S;
139  }
140 
141  // dte/dqj
142  {
143  Matrix3D S;
144  skew(S,tc);
145  Jj.template block<3,3>(0,3)=Rab*S;
146  }
147 
148  // dre/dqi
149  {
150  double buf[27];
151  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
152  Matrix3D Sxt,Syt,Szt;
153  internal::skewT(Sxt,Syt,Szt,Rbc);
154 #ifdef __clang__
155  Matrix3D temp = Rab * Sxt;
156  Eigen::Map<Matrix3D> M2(temp.data());
157  Eigen::Map<Matrix3D> Mx(buf); Mx = M2;
158  temp = Ra*Syt;
159  Eigen::Map<Matrix3D> My(buf+9); My = M2;
160  temp = Ra*Szt;
161  Eigen::Map<Matrix3D> Mz(buf+18); Mz = M2;
162 #else
163  Eigen::Map<Matrix3D> Mx(buf); Mx = Ra*Sxt;
164  Eigen::Map<Matrix3D> My(buf+9); My = Ra*Syt;
165  Eigen::Map<Matrix3D> Mz(buf+18); Mz = Ra*Szt;
166 #endif
167  Ji.template block<3,3>(3,3) = dq_dR * M;
168  }
169 
170  // dre/dqj
171  {
172  double buf[27];
173  Eigen::Map <Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
174  Matrix3D Sx,Sy,Sz;
175  internal::skew(Sx,Sy,Sz,Rc);
176 #ifdef __clang__
177  Matrix3D temp = Rab * Sx;
178  Eigen::Map<Matrix3D> M2(temp.data());
179  Eigen::Map<Matrix3D> Mx(buf); Mx = M2;
180  temp = Rab*Sy;
181  Eigen::Map<Matrix3D> My(buf+9); My = M2;
182  temp = Rab*Sz;
183  Eigen::Map<Matrix3D> Mz(buf+18); Mz = M2;
184 #else
185  Eigen::Map<Matrix3D> Mx(buf); Mx = Rab*Sx;
186  Eigen::Map<Matrix3D> My(buf+9); My = Rab*Sy;
187  Eigen::Map<Matrix3D> Mz(buf+18); Mz = Rab*Sz;
188 #endif
189  Jj.template block<3,3>(3,3) = dq_dR * M;
190  }
191  }
192 
193  template <typename Derived>
194  void computeEdgeSE3Gradient(Isometry3D& E,
195  Eigen::MatrixBase<Derived> const & JiConstRef,
196  Eigen::MatrixBase<Derived> const & JjConstRef,
197  const Isometry3D& Z,
198  const Isometry3D& Xi,
199  const Isometry3D& Xj)
200  {
201  Eigen::MatrixBase<Derived>& Ji = const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
202  Eigen::MatrixBase<Derived>& Jj = const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
203  Ji.derived().resize(6,6);
204  Jj.derived().resize(6,6);
205  // compute the error at the linearization point
206  const Isometry3D A=Z.inverse();
207  const Isometry3D B=Xi.inverse()*Xj;
208 
209  E=A*B;
210 
211  Isometry3D::ConstLinearPart Re = extractRotation(E);
212  Isometry3D::ConstLinearPart Ra = extractRotation(A);
213  Isometry3D::ConstLinearPart Rb = extractRotation(B);
214  Isometry3D::ConstTranslationPart tb = B.translation();
215 
216  Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
217  compute_dq_dR (dq_dR,
218  Re(0,0),Re(1,0),Re(2,0),
219  Re(0,1),Re(1,1),Re(2,1),
220  Re(0,2),Re(1,2),Re(2,2));
221 
222  Ji.setZero();
223  Jj.setZero();
224 
225  // dte/dti
226  Ji.template block<3,3>(0,0)=-Ra;
227 
228  // dte/dtj
229  Jj.template block<3,3>(0,0)=Re;
230 
231  // dte/dqi
232  {
233  Matrix3D S;
234  skewT(S,tb);
235  Ji.template block<3,3>(0,3)=Ra*S;
236  }
237 
238  // dte/dqj: this is zero
239 
240  double buf[27];
241  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
242  Matrix3D Sxt,Syt,Szt;
243  // dre/dqi
244  {
245  skewT(Sxt,Syt,Szt,Rb);
246  Eigen::Map<Matrix3D> Mx(buf); Mx.noalias() = Ra*Sxt;
247  Eigen::Map<Matrix3D> My(buf+9); My.noalias() = Ra*Syt;
248  Eigen::Map<Matrix3D> Mz(buf+18); Mz.noalias() = Ra*Szt;
249  Ji.template block<3,3>(3,3) = dq_dR * M;
250  }
251 
252  // dre/dqj
253  {
254  Matrix3D& Sx = Sxt;
255  Matrix3D& Sy = Syt;
256  Matrix3D& Sz = Szt;
257  skew(Sx,Sy,Sz,Matrix3D::Identity());
258  Eigen::Map<Matrix3D> Mx(buf); Mx.noalias() = Re*Sx;
259  Eigen::Map<Matrix3D> My(buf+9); My.noalias() = Re*Sy;
260  Eigen::Map<Matrix3D> Mz(buf+18); Mz.noalias() = Re*Sz;
261  Jj.template block<3,3>(3,3) = dq_dR * M;
262  }
263  }
264 
265 
266  template <typename Derived>
267  void computeEdgeSE3PriorGradient(Isometry3D& E,
268  const Eigen::MatrixBase<Derived>& JConstRef,
269  const Isometry3D& Z,
270  const Isometry3D& X,
271  const Isometry3D& P=Isometry3D())
272  {
273  Eigen::MatrixBase<Derived>& J = const_cast<Eigen::MatrixBase<Derived>&>(JConstRef);
274  J.derived().resize(6,6);
275  // compute the error at the linearization point
276  const Isometry3D A = Z.inverse()*X;
277  const Isometry3D& B = P;
278  Isometry3D::ConstLinearPart Ra = extractRotation(A);
279  Isometry3D::ConstLinearPart Rb = extractRotation(B);
280  Isometry3D::ConstTranslationPart tb = B.translation();
281  E = A*B;
282  Isometry3D::ConstLinearPart Re = extractRotation(E);
283 
284  Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
285  compute_dq_dR (dq_dR,
286  Re(0,0),Re(1,0),Re(2,0),
287  Re(0,1),Re(1,1),Re(2,1),
288  Re(0,2),Re(1,2),Re(2,2));
289 
290  J.setZero();
291 
292  // dte/dt
293  J.template block<3,3>(0,0)=Ra;
294 
295  // dte/dq =0
296  // dte/dqj
297  {
298  Matrix3D S;
299  skew(S,tb);
300  J.template block<3,3>(0,3)=Ra*S;
301  }
302 
303  // dre/dt =0
304 
305  // dre/dq
306  {
307  double buf[27];
308  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
309  Matrix3D Sx,Sy,Sz;
310  internal::skew(Sx,Sy,Sz,Rb);
311 #ifdef __clang__
312  Matrix3D temp = Ra * Sx;
313  Eigen::Map<Matrix3D> M2(temp.data());
314  Eigen::Map<Matrix3D> Mx(buf); Mx = M2;
315  temp = Ra*Sy;
316  Eigen::Map<Matrix3D> My(buf+9); My = M2;
317  temp = Ra*Sz;
318  Eigen::Map<Matrix3D> Mz(buf+18); Mz = M2;
319 #else
320  Eigen::Map<Matrix3D> Mx(buf); Mx = Ra*Sx;
321  Eigen::Map<Matrix3D> My(buf+9); My = Ra*Sy;
322  Eigen::Map<Matrix3D> Mz(buf+18); Mz = Ra*Sz;
323 #endif
324  J.template block<3,3>(3,3) = dq_dR * M;
325  }
326 
327  }
328 
329 } // end namespace internal
330 } // end namespace g2o
331 #endif
isometry3d_mappings.h
g2o_types_slam3d_api.h
g2o::internal::compute_dq_dR
void compute_dq_dR(Eigen::Matrix< double, 3, 9, Eigen::ColMajor > &dq_dR, const double &r11, const double &r21, const double &r31, const double &r12, const double &r22, const double &r32, const double &r13, const double &r23, const double &r33)
Definition: dquat2mat.cpp:42
g2o::internal::skew
void skew(Eigen::MatrixBase< Derived > &s, const Eigen::MatrixBase< DerivedOther > &v)
Definition: isometry3d_gradients.h:43
g2o::Isometry3D
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
g2o::internal::extractRotation
Isometry3D::ConstLinearPart extractRotation(const Isometry3D &A)
Definition: isometry3d_mappings.h:58
g2o::Matrix3D
Eigen::Matrix< double, 3, 3, Eigen::ColMajor > Matrix3D
Definition: eigen_types.h:61
g2o::internal::computeEdgeSE3Gradient
void computeEdgeSE3Gradient(Isometry3D &E, Eigen::MatrixBase< Derived > const &JiConstRef, Eigen::MatrixBase< Derived > const &JjConstRef, const Isometry3D &Z, const Isometry3D &Xi, const Isometry3D &Xj, const Isometry3D &Pi, const Isometry3D &Pj)
Definition: isometry3d_gradients.h:87
g2o::internal::skewT
void skewT(Eigen::MatrixBase< Derived > &s, const Eigen::MatrixBase< DerivedOther > &v)
Definition: isometry3d_gradients.h:51
g2o::internal::computeEdgeSE3PriorGradient
void computeEdgeSE3PriorGradient(Isometry3D &E, const Eigen::MatrixBase< Derived > &JConstRef, const Isometry3D &Z, const Isometry3D &X, const Isometry3D &P=Isometry3D())
Definition: isometry3d_gradients.h:267
Generated on Mon Mar 18 2019 20:14:40 for g2o by   doxygen 1.8.11