-
Notifications
You must be signed in to change notification settings - Fork 4
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Update CMakeLists.txt, build error persists.
- Loading branch information
Showing
4 changed files
with
62 additions
and
34 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,33 @@ | ||
#ifndef SE3_HPP | ||
#define SE3_HPP | ||
|
||
#include "EigenUtil.h" | ||
|
||
// Takes SE3 group object, linearly approximates rotation, returns projected matrix in real space | ||
Matrix4x4f SE3Exp(const Vector6f& twist) { | ||
Matrix4x4f M; | ||
M << 0, -twist(5), twist(4), twist(0), | ||
twist(5), 0, -twist(3), twist(1), | ||
-twist(4), twist(3), 0, twist(2), | ||
0, 0, 0, 0; | ||
return M.exp(); | ||
} | ||
|
||
// Takes SE3 Lie algebra, projects it to abstract SE3 space | ||
Vector6f SE3Log(const Matrix4x4f& transform) { | ||
Matrix4x4f M = transform.log(); | ||
Vector6f twist; | ||
twist << M(0,3), M(1,3), M(2,3), M(2,1), M(0,2), M(1,0); | ||
return twist; | ||
} | ||
|
||
// Notice. While we store rotation/translations as SE3 group elements, we need to | ||
// project them back to real space in order to compute any meaningful transform between | ||
// them because transform directly in abstract space is not possible | ||
Vector6f updateTransform(const Vector6f& perturbation, const Vector6f prev_estimate) { | ||
return SE3Log(SE3Exp(perturbation)*SE3Exp(prev_estimate)); | ||
} | ||
|
||
|
||
|
||
#endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters