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
Gepetto
aicp_mapping
Commits
839cf1bd
Commit
839cf1bd
authored
Apr 25, 2019
by
Maurice Fallon
Browse files
fix the transform. confident its correct now
parent
3a839950
Changes
3
Hide whitespace changes
Inline
Side-by-side
aicp_core/src/registration/app.cpp
View file @
839cf1bd
...
...
@@ -401,7 +401,7 @@ void App::operator()() {
Eigen
::
Isometry3d
relative_motion
=
vis_
->
getPath
().
back
().
inverse
()
*
aligned_clouds_graph_
->
getLastCloud
()
->
getCorrectedPose
();
double
dist
=
relative_motion
.
translation
().
norm
();
if
(
1
==
1
)
//
dist > 1.0)
if
(
dist
>
1.0
)
//(1==1)// use threshold to reduce number of nearby markers. this shouldnt be done here
{
vis_
->
publishPoses
(
aligned_clouds_graph_
->
getLastCloud
()
->
getCorrectedPose
(),
0
,
""
,
cloud
->
getUtime
());
...
...
@@ -412,13 +412,20 @@ void App::operator()() {
std
::
cout
<<
"odom_to_map publish <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
\n
"
;
Eigen
::
Isometry3d
base_to_odom
=
aligned_clouds_graph_
->
getLastCloud
()
->
getOdomPose
();
Eigen
::
Isometry3d
base_to_map
=
aligned_clouds_graph_
->
getLastCloud
()
->
getCorrectedPose
();
Eigen
::
Isometry3d
odom_to_base
=
aligned_clouds_graph_
->
getLastCloud
()
->
getOdomPose
();
Eigen
::
Isometry3d
map_to_base
=
aligned_clouds_graph_
->
getLastCloud
()
->
getCorrectedPose
();
Eigen
::
Isometry3d
odom_to_map
=
(
base_to_map
.
inverse
()
*
base_to_odom
).
inverse
();
std
::
cout
<<
odom_to_map
.
translation
().
transpose
()
<<
" odom_to_map publish
\n
"
;
Eigen
::
Isometry3d
odom_to_map
=
(
map_to_base
*
odom_to_base
.
inverse
()).
inverse
();
vis_
->
publishOdomToMapPose
(
odom_to_map
,
cloud
->
getUtime
());
Eigen
::
Quaterniond
q_corr
=
Eigen
::
Quaterniond
(
odom_to_map
.
rotation
());
double
r_corr
,
p_corr
,
y_corr
;
quat_to_euler
(
q_corr
,
r_corr
,
p_corr
,
y_corr
);
std
::
cout
<<
odom_to_map
.
translation
().
transpose
()
<<
" odom_to_map publish
\n
"
;
std
::
cout
<<
r_corr
*
180
/
M_PI
<<
" "
<<
p_corr
*
180
/
M_PI
<<
" "
<<
y_corr
*
180
/
M_PI
<<
" rpy
\n
"
;
}
// Store aligned map and VISUALIZE
...
...
aicp_ros/python/aicp_tf_bridge.py
View file @
839cf1bd
#!/usr/bin/env python
# example showing publishing of a pose and a point cloud
# then moving the point cloud by moving the pose ... but not republishing
# the point cloud
# this is very useful for SLAM
# link aicp's correction into tf (instead of localization manager)
import
rospy
from
geometry_msgs.msg
import
TransformStamped
...
...
aicp_ros/python/aicp_tf_filter.py
0 → 100644
View file @
839cf1bd
#!/usr/bin/env python
# link aicp's correction into tf (instead of localization manager)
import
rospy
from
tf2_msgs.msg
import
TFMessage
import
tf
import
math
import
numpy
import
time
tf_new_pub
=
rospy
.
Publisher
(
"/tf"
,
TFMessage
,
queue_size
=
10
)
def
handle_odom_to_map
(
msg
):
for
t
in
msg
.
transforms
:
if
(
t
.
child_frame_id
==
"map"
):
print
"y"
return
tf_new_pub
.
publish
(
msg
)
rospy
.
init_node
(
'aicp_tf_filter'
)
rospy
.
Subscriber
(
'/tf_old'
,
TFMessage
,
handle_odom_to_map
)
rospy
.
spin
()
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