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
fea790f6
Commit
fea790f6
authored
Apr 25, 2022
by
Joan Solà Ortega
Browse files
Update processor_visual_odometry.cpp
parent
73673fda
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/processor/processor_visual_odometry.cpp
View file @
fea790f6
...
...
@@ -486,16 +486,16 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat)
Eigen
::
Vector3d
point3d
;
point3d
=
pinhole
::
backprojectPoint
(
getSensor
()
->
getIntrinsic
()
->
getState
(),
(
std
::
static_pointer_cast
<
SensorCamera
>
(
getSensor
()))
->
getCorrectionVector
(),
point2d
);
getSensor
()
->
getIntrinsic
()
->
getState
(),
(
std
::
static_pointer_cast
<
SensorCamera
>
(
getSensor
()))
->
getCorrectionVector
(),
point2d
);
//double distance = params_bundle_adjustment_->distance; // arbitrary value
//
double distance = params_bundle_adjustment_->distance; // arbitrary value
double
distance
=
1
;
Eigen
::
Vector4d
vec_homogeneous_c
;
vec_homogeneous_c
=
{
point3d
(
0
),
point3d
(
1
),
point3d
(
2
),
point3d
.
norm
()
/
distance
};
//
TODO:
lmk from camera to world coordinate frame.
// lmk from camera to world coordinate frame.
Transform
<
double
,
3
,
Isometry
>
T_w_r
=
Translation
<
double
,
3
>
(
feat_pi
->
getFrame
()
->
getP
()
->
getState
())
*
Quaterniond
(
_feat
->
getFrame
()
->
getO
()
->
getState
().
data
());
...
...
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