Hi, I’m trying to implement visual odometry using ArUco markers to maintain hold position. I’m confused about the coordinate system axes that I need to provide to the visual odometry system and the type of reference frame to use in the topic (FRD/NED/unknown). I’ve tried several times to fly in position mode when the drone can see the marker, but the drone drifts randomly.
this is the code
```
// Extract tag position and orientation from message
Eigen::Vector3d tag_pos(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
Eigen::Quaterniond tag_quat(msg->pose.orientation.w, msg->pose.orientation.x,
msg->pose.orientation.y, msg->pose.orientation.z);
// Calculate camera position from tag
Eigen::Matrix3d tag_R_in_camera = tag_quat.toRotationMatrix();
Eigen::Vector3d camera_pos_in_tag = -(tag_R_in_camera.transpose() * tag_pos);
Eigen::Quaterniond camera_quat_in_tag = tag_quat;
// Publish camera pose from tag
Eigen::Vector3d drone_pos;
drone_pos(0) = camera_pos_in_tag(0);
drone_pos(1) = -camera_pos_in_tag(1);
drone_pos(2) = -camera_pos_in_tag(2);
Eigen::Quaterniond drone_quat;
drone_quat.x() = camera_quat_in_tag.x();
drone_quat.y() = -camera_quat_in_tag.y();
drone_quat.z() = -camera_quat_in_tag.z();
drone_quat.w() = camera_quat_in_tag.w();
publishTagPose(tag_pos, tag_quat);
publishCameraPose(camera_pos_in_tag, camera_quat_in_tag);
publishDronePose(drone_pos, drone_quat);
publishVehicleOdometry(drone_pos, drone_quat);
```
about:blank#blocked