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
2bda112d
Commit
2bda112d
authored
Mar 16, 2016
by
jcarpent
Browse files
[C++] Cholesky decomposition now handles matrices rhs operand
parent
698c2cfb
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/algorithm/cholesky.hpp
View file @
2bda112d
...
...
@@ -56,7 +56,7 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[inout] y The input
vector
to inverse which also contains the result \f$x\f$ of the inversion.
/// \param[inout] y The input
matrix
to inverse which also contains the result \f$x\f$ of the inversion.
///
template
<
typename
Mat
>
Mat
&
solve
(
const
Model
&
model
,
const
Data
&
data
,
...
...
@@ -67,7 +67,7 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input
vector
to multiply with data.M and storing the result.
/// \param[inout] v The input
matrix
to multiply with data.M and storing the result.
/// \param[in] usingCholesky If true, use the Cholesky decomposition stored in data. Exploit the sparsity of the kinematic tree.
///
/// \return A reference to the result of \f$ Mv \f$.
...
...
@@ -82,7 +82,7 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input
vector
to multiply with data.U and also storing the result.
/// \param[inout] v The input
matrix
to multiply with data.U and also storing the result.
///
/// \return A reference to the result of \f$ Uv \f$ stored in v.
///
...
...
@@ -96,7 +96,7 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input
vector
to multiply with data.U.tranpose() and also storing the result.
/// \param[inout] v The input
matrix
to multiply with data.U.tranpose() and also storing the result.
///
/// \return A reference to the result of \f$ U^{\top}v \f$ stored in v.
///
...
...
@@ -110,7 +110,7 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input
vector
to multiply with data.U^{-1} and also storing the result.
/// \param[inout] v The input
matrix
to multiply with data.U^{-1} and also storing the result.
///
/// \return A reference to the result of \f$ U^{-1}v \f$ stored in v.
///
...
...
@@ -126,7 +126,7 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input
vector
to multiply with data.U^{-\top} and also storing the result.
/// \param[inout] v The input
matrix
to multiply with data.U^{-\top} and also storing the result.
///
/// \return A reference to the result of \f$ U^{-\top}v \f$ stored in v.
///
...
...
@@ -142,7 +142,7 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input
vector
to multiply with data.M^{-1} and also storing the result.
/// \param[inout] v The input
matrix
to multiply with data.M^{-1} and also storing the result.
///
/// \return A reference to the result of \f$ M^{-1}v \f$ stored in v.
///
...
...
src/algorithm/cholesky.hxx
View file @
2bda112d
...
...
@@ -71,13 +71,13 @@ namespace se3
const
Data
&
data
,
Eigen
::
MatrixBase
<
Mat
>
&
v
)
{
assert
(
v
.
size
()
==
model
.
nv
);
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Mat
);
assert
(
v
.
rows
()
==
model
.
nv
);
const
Eigen
::
MatrixXd
&
U
=
data
.
U
;
const
std
::
vector
<
int
>
&
nvt
=
data
.
nvSubtree_fromRow
;
for
(
int
k
=
0
;
k
<
model
.
nv
-
1
;
++
k
)
// You can stop one step before nv
v
[
k
]
+=
U
.
row
(
k
).
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
)
*
v
.
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
);
v
.
row
(
k
)
+=
U
.
row
(
k
).
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
)
*
v
.
middleRows
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
);
return
v
.
derived
();
}
...
...
@@ -88,13 +88,12 @@ namespace se3
const
Data
&
data
,
Eigen
::
MatrixBase
<
Mat
>
&
v
)
{
assert
(
v
.
size
()
==
model
.
nv
);
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Mat
);
assert
(
v
.
rows
()
==
model
.
nv
);
const
Eigen
::
MatrixXd
&
U
=
data
.
U
;
const
std
::
vector
<
int
>
&
nvt
=
data
.
nvSubtree_fromRow
;
for
(
int
i
=
model
.
nv
-
2
;
i
>=
0
;
--
i
)
// You can start from nv-2 (no child in nv-1)
v
.
segment
(
i
+
1
,
nvt
[(
Model
::
Index
)
i
]
-
1
)
+=
U
.
row
(
i
).
segment
(
i
+
1
,
nvt
[(
Model
::
Index
)
i
]
-
1
).
transpose
()
*
v
[
i
]
;
for
(
int
k
=
model
.
nv
-
2
;
k
>=
0
;
--
k
)
// You can start from nv-2 (no child in nv-1)
v
.
middleRows
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
)
+=
U
.
row
(
k
).
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
).
transpose
()
*
v
.
row
(
k
)
;
return
v
.
derived
();
}
...
...
@@ -109,14 +108,13 @@ namespace se3
{
/* We search y s.t. v = U y.
* For any k, v_k = y_k + U_{k,k+1:} y_{k+1:} */
assert
(
v
.
size
()
==
model
.
nv
);
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Mat
);
assert
(
v
.
rows
()
==
model
.
nv
);
const
Eigen
::
MatrixXd
&
U
=
data
.
U
;
const
std
::
vector
<
int
>
&
nvt
=
data
.
nvSubtree_fromRow
;
for
(
int
k
=
model
.
nv
-
2
;
k
>=
0
;
--
k
)
// You can start from nv-2 (no child in nv-1)
v
[
k
]
-=
U
.
row
(
k
).
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
)
*
v
.
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
);
v
.
row
(
k
)
-=
U
.
row
(
k
).
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
)
*
v
.
middleRows
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
);
return
v
.
derived
();
}
...
...
@@ -127,13 +125,12 @@ namespace se3
{
/* We search y s.t. v = U' y.
* For any k, v_k = y_k + sum_{m \in parent{k}} U(m,k) v(k). */
assert
(
v
.
size
()
==
model
.
nv
);
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Mat
);
assert
(
v
.
rows
()
==
model
.
nv
);
const
Eigen
::
MatrixXd
&
U
=
data
.
U
;
const
std
::
vector
<
int
>
&
nvt
=
data
.
nvSubtree_fromRow
;
for
(
int
i
=
0
;
i
<
model
.
nv
-
1
;
++
i
)
// You can stop one step before nv.
v
.
segment
(
i
+
1
,
nvt
[(
Model
::
Index
)
i
]
-
1
)
-=
U
.
row
(
i
).
segment
(
i
+
1
,
nvt
[(
Model
::
Index
)
i
]
-
1
).
transpose
()
*
v
[
i
]
;
for
(
int
k
=
0
;
k
<
model
.
nv
-
1
;
++
k
)
// You can stop one step before nv.
v
.
middleRows
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
)
-=
U
.
row
(
k
).
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
).
transpose
()
*
v
.
row
(
k
)
;
return
v
.
derived
();
}
...
...
@@ -145,8 +142,7 @@ namespace se3
const
Data
&
data
,
const
Eigen
::
MatrixBase
<
Mat
>
&
v
)
{
assert
(
v
.
size
()
==
model
.
nv
);
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Mat
);
assert
(
v
.
rows
()
==
model
.
nv
);
const
Eigen
::
MatrixXd
&
M
=
data
.
M
;
const
std
::
vector
<
int
>
&
nvt
=
data
.
nvSubtree_fromRow
;
...
...
@@ -154,8 +150,8 @@ namespace se3
for
(
int
k
=
model
.
nv
-
1
;
k
>=
0
;
--
k
)
{
res
[
k
]
=
M
.
row
(
k
).
segment
(
k
,
nvt
[(
Model
::
Index
)
k
])
*
v
.
segment
(
k
,
nvt
[(
Model
::
Index
)
k
]);
res
.
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
)
+=
M
.
row
(
k
).
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
).
transpose
()
*
v
[
k
]
;
res
.
row
(
k
)
=
M
.
row
(
k
).
segment
(
k
,
nvt
[(
Model
::
Index
)
k
])
*
v
.
middleRows
(
k
,
nvt
[(
Model
::
Index
)
k
]);
res
.
middleRows
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
)
+=
M
.
row
(
k
).
segment
(
k
+
1
,
nvt
[(
Model
::
Index
)
k
]
-
1
).
transpose
()
*
v
.
row
(
k
)
;
}
return
res
;
...
...
@@ -167,7 +163,7 @@ namespace se3
Eigen
::
MatrixBase
<
Mat
>
&
v
)
{
Utv
(
model
,
data
,
v
);
for
(
int
k
=
0
;
k
<
model
.
nv
;
++
k
)
v
[
k
]
*=
data
.
D
[
k
];
for
(
int
k
=
0
;
k
<
model
.
nv
;
++
k
)
v
.
row
(
k
)
*=
data
.
D
[
k
];
return
Uv
(
model
,
data
,
v
);
}
}
// internal
...
...
@@ -188,7 +184,7 @@ namespace se3
Eigen
::
MatrixBase
<
Mat
>
&
v
)
{
Uiv
(
model
,
data
,
v
);
for
(
int
k
=
0
;
k
<
model
.
nv
;
++
k
)
v
[
k
]
/=
data
.
D
[
k
];
for
(
int
k
=
0
;
k
<
model
.
nv
;
++
k
)
v
.
row
(
k
)
/=
data
.
D
[
k
];
return
Utiv
(
model
,
data
,
v
);
}
...
...
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