### Question:
So2Pose { public So2Pose inverse() { double[][] mArray = { { Math.cos(orientation), -Math.sin(orientation), getX() }, { Math.sin(orientation), Math.cos(orientation), getY() }, { 0, 0, 1 } }; Matrix mMatrix = new Matrix(mArray); Matrix mMatrixInv = mMatrix.inverse(); double th = Util.normalizeAngleRad(Math.atan2(mMatrixInv.get(1, 0), mMatrixInv.get(0, 0))); return new So2Pose(mMatrixInv.get(0, 2), mMatrixInv.get(1, 2), th); } So2Pose(Point newLocation, double newOrientation);  So2Pose(double x, double y, double newOrientation); So2Pose mult(So2Pose postPose); So2Pose inverse(); void updatePoint(Point newPoint, double newOrientation); Point getSe2Point(); double getX(); double getY(); double getOrientation(); static So2Pose identity(); @Override int hashCode(); @Override boolean equals(Object o); String toString();  }### Answer:
@Test public void testInverse() { So2Pose a = new So2Pose(1.0, 2.0, 3.0); So2Pose b = a.inverse(); So2Pose c = b.mult(a); if (!c.equals(So2Pose.identity())) { fail("matrix inverse time original is not identity"); } So2Pose d = a.mult(b); if (!d.equals(So2Pose.identity())) { fail("matrix inverse time original is not identity"); } }