Add parameter ros_map_to_cv_map_frame #168
Conversation
|
|
||
| // Create odometry message and update it with current camera pose | ||
| nav_msgs::msg::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_msg.pose.pose = tf2::toMsg(map_to_camera_affine * rot_ros_to_cv_coordinate_.inverse()); |
There was a problem hiding this comment.
Having rot_ros_to_cv_coordinate still as a hardcoded matrix seems to go against what you try to achieve with ros_map_to_cv_map_affine.
Exposing ros_map_to_cv_map_affine to the user suggests they have control over the optical frame transform. But with this code, they are forced to always include the optical frame transform on top of whatever custom transform they want.
One of the following 2 options makes more sense to me:
- Always apply the optical frame transform in the code here (and other required places), meaning the default of ros_map_to_cv_map_affine can be the identity transform. (which probably means ros_map_to_cv_map gets renamed to ros_map_to_stella_map or something like that)
- Use camera_optical_frame_ for child_frame_id and simply drop the rot_ros_to_cv_coordinate_ entirely.
Not sure what the best approach would be. In my personal fork I'm using camera_optical_frame and I also do a tf-lookup here from camera_optical_frame to base_frame, so I can publish with child_frame_id=base_frame. The added benefit of that is that my map_offset_transform (similar to what you're trying to add here with ros_map_to_cv_map_affine_) is very intuitive. You typically would configure that with an x,y,yaw for 2d navigation and z,roll,pitch can stay zero in that case.
There was a problem hiding this comment.
The first option looks better.
| const Eigen::Vector3d normal_vector = (Eigen::Vector3d() << 0., 1., 0.).finished(); | ||
| if (!slam_->relocalize_by_pose_2d(cam_pose_cv, normal_vector)) { | ||
| const Eigen::Vector3d normal_vector = (Eigen::Vector3d() << 0., 0., 1.).finished(); | ||
| if (!slam_->relocalize_by_pose_2d(cam_pose_cv, ros_map_to_cv_map_affine_.inverse().linear() * normal_vector)) { |
There was a problem hiding this comment.
I'm not sure this is correct, unless I'm misinterpreting your intention with ros_map_to_cv_map_affine_.
My assumption was you want it to add a customizable offset between desired map frame and the initial pose when creating the map.
Do you instead intend users to specify the entire transform from base_link to camera_optical_link (on top of the desired map offset)?
- If so then I think this code is correct, but I think publish_pose then is invalid.
- If not, I think this part of the code probably needs a different transform for the normal vector, but I'm getting confused and can easily say what exactly.
Doesn't cam_pose_cv also need a transform including ros_map_to_map_affine in some way?
There was a problem hiding this comment.
Fixed in 316a220.
No description provided.