Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add parameter ros_map_to_cv_map_frame #168

Draft
wants to merge 2 commits into
base: ros2
Choose a base branch
from
Draft

Add parameter ros_map_to_cv_map_frame #168

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
dddc5fc
Add parameter ros_map_to_cv_map_frame
ymd-stella Sep 24, 2023
316a220
Fix cam_pose_cv for relocalize_by_pose_2d
ymd-stella Sep 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 16 additions & 11 deletions src/stella_vslam_ros.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,10 +41,10 @@ system::system(const std::shared_ptr<stella_vslam::system>& slam,
std::bind(&system::init_pose_callback,
this, std::placeholders::_1));
setParams();
rot_ros_to_cv_map_frame_ = (Eigen::Matrix3d() << 0, 0, 1,
-1, 0, 0,
0, -1, 0)
.finished();
rot_ros_to_cv_coordinate_ = (Eigen::Matrix3d() << 0, 0, 1,
-1, 0, 0,
0, -1, 0)
.finished();
}

void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc, const rclcpp::Time& stamp) {
Expand All @@ -54,14 +54,14 @@ void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc, const rclcpp::Time
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_);
map_to_camera_affine = ros_map_to_cv_map_affine_ * map_to_camera_affine;

// 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());
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The first option looks better.

pose_pub_->publish(pose_msg);

// Send map->odom transform. Set publish_tf to false if not using TF
Expand All @@ -74,8 +74,8 @@ void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc, const rclcpp::Time

geometry_msgs::msg::TransformStamped map_to_odom_msg;
if (odom2d_) {
Eigen::Affine3d map_to_camera_affine_2d = project_to_xy_plane(map_to_camera_affine * rot_ros_to_cv_map_frame_.inverse()) * rot_ros_to_cv_map_frame_;
Eigen::Affine3d camera_to_odom_affine_2d = (project_to_xy_plane(camera_to_odom_affine.inverse() * rot_ros_to_cv_map_frame_.inverse()) * rot_ros_to_cv_map_frame_).inverse();
Eigen::Affine3d map_to_camera_affine_2d = project_to_xy_plane(map_to_camera_affine * rot_ros_to_cv_coordinate_.inverse()) * rot_ros_to_cv_coordinate_;
Eigen::Affine3d camera_to_odom_affine_2d = (project_to_xy_plane(camera_to_odom_affine.inverse() * rot_ros_to_cv_coordinate_.inverse()) * rot_ros_to_cv_coordinate_).inverse();
Eigen::Affine3d map_to_odom_affine_2d = map_to_camera_affine_2d * camera_to_odom_affine_2d;
map_to_odom_msg = tf2::eigenToTransform(map_to_odom_affine_2d);
}
Expand Down Expand Up @@ -110,7 +110,7 @@ void system::publish_keyframes(const rclcpp::Time& stamp) {
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);
Eigen::Affine3d pose_affine = rot_ros_to_cv_map_frame_ * map_to_camera_affine * rot_ros_to_cv_map_frame_.inverse();
Eigen::Affine3d pose_affine = ros_map_to_cv_map_affine_ * map_to_camera_affine * rot_ros_to_cv_coordinate_.inverse();
keyframes_msg.poses.push_back(tf2::toMsg(pose_affine));
keyframes_2d_msg.poses.push_back(tf2::toMsg(project_to_xy_plane(pose_affine)));
}
Expand Down Expand Up @@ -142,6 +142,11 @@ void system::setParams() {

transform_tolerance_ = 0.5;
transform_tolerance_ = node_->declare_parameter("transform_tolerance", transform_tolerance_);

std::vector<double> ros_map_to_cv_map_frame{0.0, 0.0, 0.0, 0.5, -0.5, 0.5, -0.5}; // x, y, z, qx, qy, qz, qw
ros_map_to_cv_map_frame = node_->declare_parameter("ros_map_to_cv_map_frame", ros_map_to_cv_map_frame);
ros_map_to_cv_map_affine_ = Eigen::Translation<double, 3>(ros_map_to_cv_map_frame[0], ros_map_to_cv_map_frame[1], ros_map_to_cv_map_frame[2])
* Eigen::Quaterniond(ros_map_to_cv_map_frame[6], ros_map_to_cv_map_frame[3], ros_map_to_cv_map_frame[4], ros_map_to_cv_map_frame[5]);
}

void system::init_pose_callback(
Expand Down Expand Up @@ -204,8 +209,8 @@ void system::init_pose_callback(
* initialpose_affine * robot_base_frame_to_camera_affine)
.matrix();

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)) {
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fixed in 316a220.

RCLCPP_ERROR(node_->get_logger(), "Can not set initial pose");
}
}
Expand Down
4 changes: 3 additions & 1 deletion src/stella_vslam_ros.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,10 +65,12 @@ class system {
// If true, odom_frame is fixed on the xy-plane of map_frame. This is useful when working with 2D navigation modules.
bool odom2d_;

Eigen::Affine3d ros_map_to_cv_map_affine_;

private:
void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);

Eigen::AngleAxisd rot_ros_to_cv_map_frame_;
Eigen::AngleAxisd rot_ros_to_cv_coordinate_;
};

class mono : public system {
Expand Down