How to transform pose from camera frame to world frame?

[Tina-Lau]

topic _/posecam/0 outputs camera pose obviously under camera frame. This note says that "The camera position in world coordinates must be obtained by inversion". However in visualizer.cpp where topic _/posecam/0 is published, I tried to adapt inversion, but nothing changed. This is how I code: `void Visualizer::publishCameraPoses(const FrameBundlePtr& frame_bundle, const int64_t timestamp_nanoseconds) { vk::output_helper::publishTfTransform( frame_bundle->at(0)->T_cam_world(), ros::Time().fromNSec(timestamp_nanoseconds), "campos", kWorldFrame, br);

for (size_t i = 0; i < frame_bundle->size(); ++i) { // if (pub_camposes.at(i).getNumSubscribers() == 0) // return; VLOG(100) << "Publish camera pose " << i;

const FramePtr& frame = frame_bundle->at(i);
Transformation T_world_from_cam(frame->T_f_w_.inverse());
Eigen::Quaterniond q =
    T_world_from_cam.getRotation().toImplementation();
Eigen::Vector3d p = T_world_from_cam.getPosition();
geometry_msgs::PoseStampedPtr msg_pose(new geometry_msgs::PoseStamped);
msg_pose->header.seq = trace_id_;
msg_pose->header.stamp = ros::Time().fromNSec(timestamp_nanoseconds);
msg_pose->header.frame_id = "cam" + std::to_string(i);
msg_pose->pose.position.x = p[0];
msg_pose->pose.position.y = p[1];
msg_pose->pose.position.z = p[2];
msg_pose->pose.orientation.x = q.x();
msg_pose->pose.orientation.y = q.y();
msg_pose->pose.orientation.z = q.z();
msg_pose->pose.orientation.w = q.w();
pub_cam_poses_.at(i).publish(msg_pose);

} }`

And, if I get it right, its original code that uses function _T_worldcam() performed transformation already. So why the outputs were not so? And it seems like publisher of _/posecam/0 only appeared in visualizer.cpp.

How to transform the camera pose to world frame? Which file should I edit? Or which topic should I look up to?