Commit 839cf1bd authored by Maurice Fallon's avatar Maurice Fallon
Browse files

fix the transform. confident its correct now

parent 3a839950
......@@ -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
......
#!/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
......
#!/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()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment