g2o
Loading...
Searching...
No Matches
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 <Eigen/Core>
31
32#include "dquat2mat.h"
33#include "isometry3d_mappings.h"
34
35namespace g2o {
36namespace internal {
37// forward declaration
38/* void G2O_TYPES_SLAM3D_API compute_dq_dR (Eigen::Matrix<double, 3 , 9,
39 * Eigen::ColMajor>& dq_dR , const double& r11 , const double& r21 ,
40 * const double& r31 , const double& r12 , const double& r22 , const
41 * double& r32 , const double& r13 , const double& r23 , const
42 * double& r33 ); */
43
44template <typename Derived, typename DerivedOther, bool transposed = false>
45inline void skew(Eigen::MatrixBase<Derived>& s,
46 const Eigen::MatrixBase<DerivedOther>& v) {
47 const double x = 2 * v(0);
48 const double y = 2 * v(1);
49 const double z = 2 * v(2);
50 if (transposed)
51 s << 0., -z, y, z, 0, -x, -y, x, 0;
52 else
53 s << 0., z, -y, -z, 0, x, y, -x, 0;
54}
55
56template <typename Derived, typename DerivedOther>
57inline void skewT(Eigen::MatrixBase<Derived>& s,
58 const Eigen::MatrixBase<DerivedOther>& v) {
59 skew<Derived, DerivedOther, true>(s, v);
60}
61
62template <typename Derived, typename DerivedOther, bool transposed = false>
63void skew(Eigen::MatrixBase<Derived>& Sx, Eigen::MatrixBase<Derived>& Sy,
64 Eigen::MatrixBase<Derived>& Sz,
65 const Eigen::MatrixBase<DerivedOther>& R) {
66 const double r11 = 2 * R(0, 0), r12 = 2 * R(0, 1), r13 = 2 * R(0, 2),
67 r21 = 2 * R(1, 0), r22 = 2 * R(1, 1), r23 = 2 * R(1, 2),
68 r31 = 2 * R(2, 0), r32 = 2 * R(2, 1), r33 = 2 * R(2, 2);
69 if (transposed) {
70 Sx << 0, 0, 0, r31, r32, r33, -r21, -r22, -r23;
71 Sy << -r31, -r32, -r33, 0, 0, 0, r11, r12, r13;
72 Sz << r21, r22, r23, -r11, -r12, -r13, 0, 0, 0;
73 } else {
74 Sx << 0, 0, 0, -r31, -r32, -r33, r21, r22, r23;
75 Sy << r31, r32, r33, 0, 0, 0, -r11, -r12, -r13;
76 Sz << -r21, -r22, -r23, r11, r12, r13, 0, 0, 0;
77 }
78}
79
80template <typename Derived, typename DerivedOther>
81inline void skewT(Eigen::MatrixBase<Derived>& Sx,
82 Eigen::MatrixBase<Derived>& Sy,
83 Eigen::MatrixBase<Derived>& Sz,
84 const Eigen::MatrixBase<DerivedOther>& R) {
85 skew<Derived, DerivedOther, true>(Sx, Sy, Sz, R);
86}
87
88template <typename Derived>
89void computeEdgeSE3Gradient(Isometry3& E,
90 Eigen::MatrixBase<Derived> const& JiConstRef,
91 Eigen::MatrixBase<Derived> const& JjConstRef,
92 const Isometry3& Z, const Isometry3& Xi,
93 const Isometry3& Xj,
94 const Isometry3& Pi /*=Isometry3()*/,
95 const Isometry3& Pj /*=Isometry3()*/) {
96 Eigen::MatrixBase<Derived>& Ji =
97 const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
98 Eigen::MatrixBase<Derived>& Jj =
99 const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
100 Ji.derived().resize(6, 6);
101 Jj.derived().resize(6, 6);
102 // compute the error at the linearization point
103 const Isometry3 A = Z.inverse() * Pi.inverse();
104 const Isometry3 B = Xi.inverse() * Xj;
105 const Isometry3& C = Pj;
106
107 const Isometry3 AB = A * B;
108 const Isometry3 BC = B * C;
109 E = AB * C;
110
111 Isometry3::ConstLinearPart Re = extractRotation(E);
112 Isometry3::ConstLinearPart Ra = extractRotation(A);
113 // const Matrix3 Rb = extractRotation(B);
114 Isometry3::ConstLinearPart Rc = extractRotation(C);
115 Isometry3::ConstTranslationPart tc = C.translation();
116 // Isometry3::ConstTranslationParttab=AB.translation();
117 Isometry3::ConstLinearPart Rab = extractRotation(AB);
118 Isometry3::ConstTranslationPart tbc = BC.translation();
119 Isometry3::ConstLinearPart Rbc = extractRotation(BC);
120
121 Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
122 compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
123 Re(2, 1), 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 Matrix3 S;
137 skewT(S, tbc);
138 Ji.template block<3, 3>(0, 3) = Ra * S;
139 }
140
141 // dte/dqj
142 {
143 Matrix3 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 Matrix3 Sxt, Syt, Szt;
153 internal::skewT(Sxt, Syt, Szt, Rbc);
154#ifdef __clang__
155 Matrix3 temp = Rab * Sxt;
156 Eigen::Map<Matrix3> M2(temp.data());
157 Eigen::Map<Matrix3> Mx(buf);
158 Mx = M2;
159 temp = Ra * Syt;
160 Eigen::Map<Matrix3> My(buf + 9);
161 My = M2;
162 temp = Ra * Szt;
163 Eigen::Map<Matrix3> Mz(buf + 18);
164 Mz = M2;
165#else
166 Eigen::Map<Matrix3> Mx(buf);
167 Mx = Ra * Sxt;
168 Eigen::Map<Matrix3> My(buf + 9);
169 My = Ra * Syt;
170 Eigen::Map<Matrix3> Mz(buf + 18);
171 Mz = Ra * Szt;
172#endif
173 Ji.template block<3, 3>(3, 3) = dq_dR * M;
174 }
175
176 // dre/dqj
177 {
178 double buf[27];
179 Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
180 Matrix3 Sx, Sy, Sz;
181 internal::skew(Sx, Sy, Sz, Rc);
182#ifdef __clang__
183 Matrix3 temp = Rab * Sx;
184 Eigen::Map<Matrix3> M2(temp.data());
185 Eigen::Map<Matrix3> Mx(buf);
186 Mx = M2;
187 temp = Rab * Sy;
188 Eigen::Map<Matrix3> My(buf + 9);
189 My = M2;
190 temp = Rab * Sz;
191 Eigen::Map<Matrix3> Mz(buf + 18);
192 Mz = M2;
193#else
194 Eigen::Map<Matrix3> Mx(buf);
195 Mx = Rab * Sx;
196 Eigen::Map<Matrix3> My(buf + 9);
197 My = Rab * Sy;
198 Eigen::Map<Matrix3> Mz(buf + 18);
199 Mz = Rab * Sz;
200#endif
201 Jj.template block<3, 3>(3, 3) = dq_dR * M;
202 }
203}
204
205template <typename Derived>
206void computeEdgeSE3Gradient(Isometry3& E,
207 Eigen::MatrixBase<Derived> const& JiConstRef,
208 Eigen::MatrixBase<Derived> const& JjConstRef,
209 const Isometry3& Z, const Isometry3& Xi,
210 const Isometry3& Xj) {
211 Eigen::MatrixBase<Derived>& Ji =
212 const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
213 Eigen::MatrixBase<Derived>& Jj =
214 const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
215 Ji.derived().resize(6, 6);
216 Jj.derived().resize(6, 6);
217 // compute the error at the linearization point
218 const Isometry3 A = Z.inverse();
219 const Isometry3 B = Xi.inverse() * Xj;
220
221 E = A * B;
222
223 Isometry3::ConstLinearPart Re = extractRotation(E);
224 Isometry3::ConstLinearPart Ra = extractRotation(A);
225 Isometry3::ConstLinearPart Rb = extractRotation(B);
226 Isometry3::ConstTranslationPart tb = B.translation();
227
228 Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
229 compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
230 Re(2, 1), Re(0, 2), Re(1, 2), Re(2, 2));
231
232 Ji.setZero();
233 Jj.setZero();
234
235 // dte/dti
236 Ji.template block<3, 3>(0, 0) = -Ra;
237
238 // dte/dtj
239 Jj.template block<3, 3>(0, 0) = Re;
240
241 // dte/dqi
242 {
243 Matrix3 S;
244 skewT(S, tb);
245 Ji.template block<3, 3>(0, 3) = Ra * S;
246 }
247
248 // dte/dqj: this is zero
249
250 double buf[27];
251 Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
252 Matrix3 Sxt, Syt, Szt;
253 // dre/dqi
254 {
255 skewT(Sxt, Syt, Szt, Rb);
256 Eigen::Map<Matrix3> Mx(buf);
257 Mx.noalias() = Ra * Sxt;
258 Eigen::Map<Matrix3> My(buf + 9);
259 My.noalias() = Ra * Syt;
260 Eigen::Map<Matrix3> Mz(buf + 18);
261 Mz.noalias() = Ra * Szt;
262 Ji.template block<3, 3>(3, 3) = dq_dR * M;
263 }
264
265 // dre/dqj
266 {
267 Matrix3& Sx = Sxt;
268 Matrix3& Sy = Syt;
269 Matrix3& Sz = Szt;
270 skew(Sx, Sy, Sz, Matrix3::Identity());
271 Eigen::Map<Matrix3> Mx(buf);
272 Mx.noalias() = Re * Sx;
273 Eigen::Map<Matrix3> My(buf + 9);
274 My.noalias() = Re * Sy;
275 Eigen::Map<Matrix3> Mz(buf + 18);
276 Mz.noalias() = Re * Sz;
277 Jj.template block<3, 3>(3, 3) = dq_dR * M;
278 }
279}
280
281template <typename Derived>
282void computeEdgeSE3PriorGradient(Isometry3& E,
283 const Eigen::MatrixBase<Derived>& JConstRef,
284 const Isometry3& Z, const Isometry3& X,
285 const Isometry3& P = Isometry3::Identity()) {
286 Eigen::MatrixBase<Derived>& J =
287 const_cast<Eigen::MatrixBase<Derived>&>(JConstRef);
288 J.derived().resize(6, 6);
289 // compute the error at the linearization point
290 const Isometry3 A = Z.inverse() * X;
291 const Isometry3& B = P;
292 Isometry3::ConstLinearPart Ra = extractRotation(A);
293 Isometry3::ConstLinearPart Rb = extractRotation(B);
294 Isometry3::ConstTranslationPart tb = B.translation();
295 E = A * B;
296 Isometry3::ConstLinearPart Re = extractRotation(E);
297
298 Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
299 compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
300 Re(2, 1), Re(0, 2), Re(1, 2), Re(2, 2));
301
302 J.setZero();
303
304 // dte/dt
305 J.template block<3, 3>(0, 0) = Ra;
306
307 // dte/dq =0
308 // dte/dqj
309 {
310 Matrix3 S;
311 skew(S, tb);
312 J.template block<3, 3>(0, 3) = Ra * S;
313 }
314
315 // dre/dt =0
316
317 // dre/dq
318 {
319 double buf[27];
320 Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
321 Matrix3 Sx, Sy, Sz;
322 internal::skew(Sx, Sy, Sz, Rb);
323#ifdef __clang__
324 Matrix3 temp = Ra * Sx;
325 Eigen::Map<Matrix3> M2(temp.data());
326 Eigen::Map<Matrix3> Mx(buf);
327 Mx = M2;
328 temp = Ra * Sy;
329 Eigen::Map<Matrix3> My(buf + 9);
330 My = M2;
331 temp = Ra * Sz;
332 Eigen::Map<Matrix3> Mz(buf + 18);
333 Mz = M2;
334#else
335 Eigen::Map<Matrix3> Mx(buf);
336 Mx = Ra * Sx;
337 Eigen::Map<Matrix3> My(buf + 9);
338 My = Ra * Sy;
339 Eigen::Map<Matrix3> Mz(buf + 18);
340 Mz = Ra * Sz;
341#endif
342 J.template block<3, 3>(3, 3) = dq_dR * M;
343 }
344}
345
346} // end namespace internal
347} // end namespace g2o
348#endif
isometry3d_mappings.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:71
g2o::internal::skewT
void skewT(Eigen::MatrixBase< Derived > &s, const Eigen::MatrixBase< DerivedOther > &v)
Definition isometry3d_gradients.h:57
g2o::internal::computeEdgeSE3Gradient
void computeEdgeSE3Gradient(Isometry3 &E, Eigen::MatrixBase< Derived > const &JiConstRef, Eigen::MatrixBase< Derived > const &JjConstRef, const Isometry3 &Z, const Isometry3 &Xi, const Isometry3 &Xj, const Isometry3 &Pi, const Isometry3 &Pj)
Definition isometry3d_gradients.h:89
g2o::internal::computeEdgeSE3PriorGradient
void computeEdgeSE3PriorGradient(Isometry3 &E, const Eigen::MatrixBase< Derived > &JConstRef, const Isometry3 &Z, const Isometry3 &X, const Isometry3 &P=Isometry3::Identity())
Definition isometry3d_gradients.h:282
g2o::internal::skew
void skew(Eigen::MatrixBase< Derived > &s, const Eigen::MatrixBase< DerivedOther > &v)
Definition isometry3d_gradients.h:45
g2o::internal::extractRotation
Isometry3::ConstLinearPart extractRotation(const Isometry3 &A)
Definition isometry3d_mappings.h:55
g2o::Matrix3
MatrixN< 3 > Matrix3
Definition eigen_types.h:72
g2o::Isometry3
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3
Definition eigen_types.h:77
Generated on Tue Nov 11 2025 05:18:45 for g2o by doxygen 1.9.8