fix the transform. confident its correct now

aicp_core/src/registration/app.cpp

@@ -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

 #!/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

+#!/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()