For a drone application I'm trying to get the drone position in the tag frame reference, but ignoring the X & Z axis rotation. The tag is positionned vertically, and I want to ignore rotation of Z and X axis
The goal is get the drone position that points forward the tag orthogonally the Y axis whatever the other orientation of other axis.
I'm a noob in rotation matrix, I really begin!
I'm using Java and April Tag V3, but it should be the same in AruCo Tags.
I'm trying this kind of things without success
//Pose in Camera Frame (rotMatrix & trans are raw data form April Tag)
Vector3D vectorPosCamFrame = new Vector3D(trans[0][0], trans[0][1], Math.abs(trans[0][2])); //Z axis can to be only positive
Rotation rotationTag = new Rotation(rotMatrix, 1);
//Determining rotation whtout Y Axis rotation
Vector3D vectorPosCamEstimateYAxisTmp = new Vector3D(0, 1, 0);
Vector3D vectorPosCamEstimateYAxisTmpRotated = rotationTag.applyTo(vectorPosCamEstimateYAxisTmp);
Rotation rotationWithoutYAxis = new Rotation(vectorPosCamEstimateYAxisTmp, vectorPosCamEstimateYAxisTmpRotated);
//Apply rotation to current position to get position in TAG Frame
Vector3D vectorPosTagFrame = rotationTag.applyTo(vectorPosCamFrame);
vectorPosTagFrame = rotationWithoutYAxis.applyInverseTo(vectorPosCamFrame);
Does any body can explain me what I'm doing wrong?