Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
mobile_robotics
wolf_projects
wolf_lib
plugins
vision
Commits
e7047151
Commit
e7047151
authored
Apr 24, 2022
by
Joan Solà Ortega
Browse files
Minor style improvements and doc
parent
00f9087c
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/vision/factor/factor_pixel_hp.h
View file @
e7047151
...
...
@@ -130,13 +130,19 @@ inline void FactorPixelHp::expectation(const T* const _frame_p,
{
using
namespace
Eigen
;
// frames
// frames
w: world; r: robot; c: camera
Matrix
<
T
,
3
,
1
>
p_wr
(
_frame_p
);
Quaternion
<
T
>
q_wr
(
_frame_o
);
Matrix
<
T
,
3
,
1
>
p_rc
(
_sensor_p
);
Quaternion
<
T
>
q_rc
(
_sensor_o
);
// camera pose in world frame: transforms from camera to world
Matrix
<
T
,
3
,
1
>
p_wc
=
p_wr
+
q_wr
*
p_rc
;
Quaternion
<
T
>
q_wc
=
q_wr
*
q_rc
;
// invert transform: from world to camera
// | R T |.inv = | R.tr -R.tr*T | == | q.conj -q.conj*T |
// | 0 1 | | 0 1 | | 0 1 |
Quaternion
<
T
>
q_cw
=
q_wc
.
conjugate
();
Matrix
<
T
,
3
,
1
>
p_cw
=
-
(
q_cw
*
p_wc
);
...
...
@@ -148,7 +154,7 @@ inline void FactorPixelHp::expectation(const T* const _frame_p,
* | q T | * | v | = | q*v + T+w | --> v' = q*v + T+w
* | 0 1 | | w | | 0 w |
*/
Matrix
<
T
,
3
,
1
>
v_dir
=
q_cw
*
lh_w
.
template
head
<
3
>()
+
p_cw
*
lh_w
(
3
);
Matrix
<
T
,
3
,
1
>
v_dir
=
q_cw
*
lh_w
.
template
head
<
3
>()
+
p_cw
*
lh_w
(
3
);
// project point and exit
Eigen
::
Map
<
Eigen
::
Matrix
<
T
,
2
,
1
>
>
expectation
(
_expectation
);
...
...
include/vision/math/pinhole_tools.h
View file @
e7047151
...
...
@@ -272,8 +272,7 @@ Matrix<typename Derived2::Scalar, 2, 1> distortPoint(const MatrixBase<Derived1>
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Derived1
);
MatrixSizeCheck
<
2
,
1
>::
check
(
up
);
SizeEigen
n
=
d
.
size
();
if
(
n
==
0
)
if
(
d
.
size
()
==
0
)
return
up
;
else
{
typename
Derived2
::
Scalar
r2
=
up
(
0
)
*
up
(
0
)
+
up
(
1
)
*
up
(
1
);
// this is the norm squared: r2 = ||u||^2
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a 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