can anyone explain publish_pose function, in detail?

[Basavaraj-PN]

void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc, const ros::Time& stamp) {
    // Extract rotation matrix and translation vector from
    Eigen::Matrix3d rot(cam_pose_wc.block<3, 3>(0, 0));
    Eigen::Translation3d trans(cam_pose_wc.block<3, 1>(0, 3));
    Eigen::Affine3d map_to_camera_affine(trans * rot);

    // Transform map frame from CV coordinate system to ROS coordinate system
    map_to_camera_affine.prerotate(rot_ros_to_cv_map_frame_);

    // Create odometry message and update it with current camera pose
    nav_msgs::Odometry pose_msg;
    pose_msg.header.stamp = stamp;
    pose_msg.header.frame_id = map_frame_;
    pose_msg.child_frame_id = camera_frame_;
    pose_msg.pose.pose = tf2::toMsg(map_to_camera_affine * rot_ros_to_cv_map_frame_.inverse());
    pose_pub_.publish(pose_msg);

    // Send map->odom transform. Set publish_tf to false if not using TF
    if (publish_tf_) {
        try {
            auto camera_to_odom = tf_->lookupTransform(camera_optical_frame_, odom_frame_, stamp, ros::Duration(0.0));
            Eigen::Affine3d camera_to_odom_affine = tf2::transformToEigen(camera_to_odom.transform);

            auto map_to_odom_msg = tf2::eigenToTransform(map_to_camera_affine * camera_to_odom_affine);
            auto transform_timestamp = stamp + ros::Duration(transform_tolerance_);
            map_to_odom_msg.header.stamp = transform_timestamp;
            map_to_odom_msg.header.frame_id = map_frame_;
            map_to_odom_msg.child_frame_id = odom_frame_;
            map_to_odom_broadcaster_->sendTransform(map_to_odom_msg);
        }
        catch (tf2::TransformException& ex) {
            ROS_ERROR("Transform failed: %s", ex.what());
        }
    }
}```

[AlejandroSilvestri]

Please state what part of it you are interested in. I'll try to explain the first part.

Pose is a 4x4 matrix representing an euclidean transformation in homogeneous coordinates, also known as rototranslation.
It is used for change from one reference system to another, like you have this 3D point landmark in the Camera reference system and you want it expressed in the World reference system, so you multiply:

point_w = pose_wc * point_c

point_w and point_c are 4 dimensions vectors because they are in homogeneous coordinates (you add a 1 to the new dimension), an equivalent representation in the associated projective 4D space.

Pose matrices have a 3x3 rotation submatrix, and a 4x1 translation column (last column) whose last element is always 1, you discard it by taking the first 3 elements. This explains rot and trans variables.

The usual camera reference system, know as CV system, has x to the right, y bottom and z forward. ROS uses another reference system. To change between these two reference system you can use an affine3D transformation, very similar to the euclidean transformation: by definition affine let's you swap axis in ways that euclidean can't. In practice both can be seen as a 4x4 matrix with the same properties. I'm not sure if this is what is going on in the code.

[Inshu32]

Hi @Basavaraj-PN, where is this code ? I m trying to get image poses from stella vslam run but unabel to find the relevant code. Any help is welcome.