public void drawBox(BufferedImage image, RigidBodyTransform transform, double scale) { DenseMatrix64F rotation = new DenseMatrix64F(3, 3); Vector3d translation = new Vector3d(); transform.getRotation(rotation); transform.getTranslation(translation); Se3_F64 targetToSensor = new Se3_F64( rotation, new Vector3D_F64(translation.getX(), translation.getY(), translation.getZ())); Graphics2D g2 = image.createGraphics(); g2.setStroke(new BasicStroke(4)); g2.setColor(java.awt.Color.RED); VisualizeFiducial.drawCube(targetToSensor, this.intrinsicParameters, scale, 2, g2); g2.finalize(); }