Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Gabriele Buondonno
pinocchio
Commits
bf00b109
Commit
bf00b109
authored
Sep 27, 2017
by
jcarpent
Browse files
[Spatial] Make skew functions compatible with block structures
parent
28cfaad1
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/spatial/skew.hpp
View file @
bf00b109
...
...
@@ -87,7 +87,7 @@ namespace se3
/// \return The square cross product C matrix.
///
template
<
typename
D1
,
typename
D2
>
inline
Eigen
::
Matrix
<
typename
D1
::
Scalar
,
3
,
3
,
D1
::
Options
>
inline
Eigen
::
Matrix
<
typename
D1
::
Scalar
,
3
,
3
,
Eigen
::
internal
::
plain_matrix_type
<
D1
>::
type
::
Options
>
skewSquare
(
const
Eigen
::
MatrixBase
<
D1
>
&
u
,
const
Eigen
::
MatrixBase
<
D2
>
&
v
)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
D1
,
3
);
...
...
@@ -96,20 +96,20 @@ namespace se3
typedef
Eigen
::
DiagonalMatrix
<
typename
D1
::
Scalar
,
3
>
DiagonalMatrix
;
const
typename
D1
::
Scalar
udotv
(
u
.
dot
(
v
));
Eigen
::
Matrix
<
typename
D1
::
Scalar
,
3
,
3
,
D1
::
Options
>
res
(
v
*
u
.
transpose
());
Eigen
::
Matrix
<
typename
D1
::
Scalar
,
3
,
3
,
Eigen
::
internal
::
plain_matrix_type
<
D1
>::
type
::
Options
>
res
(
v
*
u
.
transpose
());
res
-=
DiagonalMatrix
(
udotv
,
udotv
,
udotv
);
return
res
;
}
template
<
typename
V
,
typename
M
>
inline
Eigen
::
Matrix
<
typename
M
::
Scalar
,
3
,
M
::
ColsAtCompileTime
,
M
::
Options
>
inline
Eigen
::
Matrix
<
typename
M
::
Scalar
,
3
,
M
::
ColsAtCompileTime
,
Eigen
::
internal
::
plain_matrix_type
<
M
>::
type
::
Options
>
cross
(
const
Eigen
::
MatrixBase
<
V
>
&
v
,
const
Eigen
::
MatrixBase
<
M
>
&
m
)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
V
,
3
);
Eigen
::
Matrix
<
typename
M
::
Scalar
,
3
,
M
::
ColsAtCompileTime
,
M
::
Options
>
res
(
3
,
m
.
cols
());
Eigen
::
Matrix
<
typename
M
::
Scalar
,
3
,
M
::
ColsAtCompileTime
,
Eigen
::
internal
::
plain_matrix_type
<
M
>::
type
::
Options
>
res
(
3
,
m
.
cols
());
res
.
row
(
0
)
=
v
[
1
]
*
m
.
row
(
2
)
-
v
[
2
]
*
m
.
row
(
1
);
res
.
row
(
1
)
=
v
[
2
]
*
m
.
row
(
0
)
-
v
[
0
]
*
m
.
row
(
2
);
res
.
row
(
2
)
=
v
[
0
]
*
m
.
row
(
1
)
-
v
[
1
]
*
m
.
row
(
0
);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment