Using visual landmark to set tf from world to odom

edit

I am using ros indigo with tf for a robot navigation problem. I am new to using tf so I am a bit confused. My robot has a standard encoders/IMU setup and I am using ekf_localization_node to compute my odom frame. That all works great.

My problem is that I have a visual localization system based on aprilTags which occasionally provides a very accurate base_link position with respect to the world frame.

My first thought was to let the aprilTag detection set odom->world as if it were static in between sightings since the aprilTag is the only link between odom->world. The visual localization provides the robot's pose so I tied to transform the localization pose back to the odom frame and then use that to compute the world->odom transform. Then I just publish that transform until a new observation updates it.

Here is my code: (visualLoactionMsg has the robots pose with respect to world).

geometry_msgs::PoseStamped transformedPose;
  geometry_msgs::PoseStamped observedPose;
  observedPose.pose = visualLocationMsg.pose.pose;
  observedPose.header.stamp = ros::Time(0);
  observedPose.header.frame_id = name+"/base_link";
  try{
    listener.transformPose(name+"/odom", observedPose, transformedPose);
    lastTransform.setOrigin(
        tf::Vector3(
          transformedPose.pose.position.x,
          transformedPose.pose.position.y,
          transformedPose.pose.position.z
          ));
    lastTransform.setRotation(
        tf::Quaternion(
          transformedPose.pose.orientation.x,
          transformedPose.pose.orientation.y,
          transformedPose.pose.orientation.z,
          transformedPose.pose.orientation.w)
          );
    lastTransform = lastTransform.inverse();
  }
...
  broadcaster.sendTransform(tf::StampedTransform(
        lastTransform, ros::Time::now(),
        "/world", name+"/odom")
      );

This seems to transform the position correctly, but not the orientation.

This may be in a tutorial somewhere, but I have been struggling with it for a while and figured I would see if anyone has a straightforward approach or could direct me to a working example. any advise would be appreciated.

Thanks, Jonathan West