geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id ="map";
cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); // Rotation information
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); // translation information
vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc);
tf::Transform new_transform;
new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));
tf::Quaternion quaternion(q[0], q[1], q[2], q[3]);
new_transform.setRotation(quaternion);
tf::poseTFToMsg(new_transform, pose.pose);
pose_pub.publish(pose);
and the current map points like in #745 (comment)
sensor_msgs::PointCloud cloud;
cloud.header.frame_id = "camera_link";
std::vector<geometry_msgs::Point32> geo_points;
std::vector<ORB_SLAM2::MapPoint*> points = mpSLAM->GetTrackedMapPoints();
for (std::vector::size_type i = 0; i != points.size(); i++) {
    if (points[i]) {
        cv::Mat coords = points[i]->GetWorldPos();
        geometry_msgs::Point32 pt;
        pt.x = coords.at<float>(2);
        pt.y = -coords.at<float>(0);
        pt.z = -coords.at<float>(1);
        geo_points.push_back(pt);
    } else {
cloud.points = geo_points;
cloud_pub.publish(cloud);
When I move the camera, the current map points (visualized in RVIZ) seem reasonable while the pose behaves weird/not correct.

It seems like the pose is not transformed correctly corresponding to the map points.
Those anybody has an idea?

Thanks a lot!:)
          
Solved it. For those who might face the same issue:

(based on https://github.com/appliedAI-Initiative/orb_slam_2_ros)

Replay the pose calculation from above with: (in ros_stereo.cc) with:
cv::Mat position = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
    if (!position.empty()) {
      tf::Transform transform = TransformFromMat (position);
      static tf::TransformBroadcaster tf_broadcaster;
      tf_broadcaster.sendTransform(tf::StampedTransform(transform, current_frame_time_, "map", "camera_link"));
      tf::Transform grasp_tf = TransformFromMat (position);
      tf::Stamped<tf::Pose> grasp_tf_pose(grasp_tf, current_frame_time_, "map");
      geometry_msgs::PoseStamped pose_msg;
      tf::poseStampedTFToMsg (grasp_tf_pose, pose_msg);
      pose_publisher_.publish(pose_msg);
where TransformFromMat() is
tf::Transform TransformFromMat (cv::Mat position_mat) {
  cv::Mat rotation(3,3,CV_32F);
  cv::Mat translation(3,1,CV_32F);
  rotation = position_mat.rowRange(0,3).colRange(0,3);
  translation = position_mat.rowRange(0,3).col(3);
  tf::Matrix3x3 tf_camera_rotation (rotation.at<float> (0,0), rotation.at<float> (0,1), rotation.at<float> (0,2),
                                    rotation.at<float> (1,0), rotation.at<float> (1,1), rotation.at<float> (1,2),
                                    rotation.at<float> (2,0), rotation.at<float> (2,1), rotation.at<float> (2,2)
  tf::Vector3 tf_camera_translation (translation.at<float> (0), translation.at<float> (1), translation.at<float> (2));
  //Coordinate transformation matrix from orb coordinate system to ros coordinate system
  const tf::Matrix3x3 tf_orb_to_ros (0, 0, 1,
                                    -1, 0, 0,
                                     0,-1, 0);
  //Transform from orb coordinate system to ros coordinate system on camera coordinates
  tf_camera_rotation = tf_orb_to_ros*tf_camera_rotation;
  tf_camera_translation = tf_orb_to_ros*tf_camera_translation;
  //Inverse matrix
  tf_camera_rotation = tf_camera_rotation.transpose();
  tf_camera_translation = -(tf_camera_rotation*tf_camera_translation);
  //Transform from orb coordinate system to ros coordinate system on map coordinates
  tf_camera_rotation = tf_orb_to_ros*tf_camera_rotation;
  tf_camera_translation = tf_orb_to_ros*tf_camera_translation;
  return tf::Transform (tf_camera_rotation, tf_camera_translation);