private Pose generateDefaultInstance() { return Pose.newBuilder() .setRotation(Rotation.newBuilder().setQw(1).setQx(0).setQy(0).setQz(0)) .setTranslation(Translation.newBuilder().setX(0).setY(0).setZ(0)).build(); } }
/** * Transforms the argument to root coordinates. * * @param translation translation that shall be transformed. * @return The transformed <code>translation</code> argument. */ private Translation transformPosition(Translation translation) { Translation.Builder transformedTranslation = translation.toBuilder().clone().clearX().clearY().clearZ(); if (translation.hasX() && translation.hasY() && translation.hasZ()) { Point3d point = new Point3d(-translation.getX(), -translation.getY(), translation.getZ()); transform.transform(point); transformedTranslation.setX(point.x).setY(point.y).setZ(point.z); } return transformedTranslation.build(); }
public static Transform3D transform(final PoseType.Pose position) { RotationType.Rotation pRotation = position.getRotation(); TranslationType.Translation pTranslation = position.getTranslation(); Quat4d jRotation = new Quat4d(pRotation.getQx(), pRotation.getQy(), pRotation.getQz(), pRotation.getQw()); Vector3d jTranslation = new Vector3d(pTranslation.getX(), pTranslation.getY(), pTranslation.getZ()); return new Transform3D(jRotation, jTranslation, 1.0); } }
/** * Updates the placement config based on the minimal coordinates and * dimensions of the bounding box. * * @param placementConfig the PlacementConfig to be updated. * @param minPosition the minimal coordinates of the new bounding box. * @param dimensions the dimensions of the bounding box (x: width, y: depth, * z: height). * @return the updated PlacementConfig including new Position, zero Rotation * and the updated bounding box. */ private static PlacementConfig updatePlacementConfig(PlacementConfig placementConfig, Point3d minPosition, Point3d dimensions) { PlacementConfig.Builder newBuilder = placementConfig.toBuilder(); newBuilder.setShape(newBuilder.getShapeBuilder() .setBoundingBox(AxisAlignedBoundingBox3DFloat.newBuilder() .setLeftFrontBottom(Translation.newBuilder().setX(0).setY(0).setZ(0)) .setWidth((float) dimensions.x) .setDepth((float) dimensions.y) .setHeight((float) dimensions.z))) .setPosition(Pose.newBuilder() .setRotation(Rotation.newBuilder().setQw(1).setQx(0).setQy(0).setQz(0)) .setTranslation(Translation.newBuilder() .setX(minPosition.x) .setY(minPosition.y) .setZ(minPosition.z))); return newBuilder.build(); }
/** * Transforms the argument to root coordinates. * * @param rotation rotation that shall be transformed. * @return The <code>rotation</code> argument. */ private Rotation transformRotation(Rotation rotation) { Rotation.Builder transformedRotation = rotation.toBuilder().clone().clearQw().clearQx().clearQy().clearQz(); if (rotation.hasQw() && rotation.hasQx() && rotation.hasQy() && rotation.hasQz()) { Transform3D t = new Transform3D(); t.set(new Quat4d(rotation.getQw(), rotation.getQx(), rotation.getQy(), rotation.getQz())); Transform3D multiply = new Transform3D(); multiply.mul(transform, t); Quat4d result = new Quat4d(); multiply.get(result); transformedRotation.setQw(result.w).setQx(result.x).setQy(result.y).setQz(result.z); } return transformedRotation.build(); } }
/** * Returns the corners of the given bounding box in local coordinates. * * @param boundingBox The bounding box. * @return The corners of the bounding box in local coordinates. */ private static List<Point3d> getCorners(AxisAlignedBoundingBox3DFloat boundingBox) { final List<Point3d> corners = new ArrayList<>(); Translation lfb = boundingBox.getLeftFrontBottom(); Point3d leftFrontBottom = new Point3d(lfb.getX(), lfb.getY(), lfb.getZ()); for (boolean w : BOOLEAN_VALUES) { for (boolean d : BOOLEAN_VALUES) { for (boolean h : BOOLEAN_VALUES) { Point3d p = new Point3d(leftFrontBottom); if (w) { p.x += boundingBox.getWidth(); } if (d) { p.y += boundingBox.getDepth(); } if (h) { p.z += boundingBox.getHeight(); } corners.add(p); } } } return corners; }
public static final Translation toTranslation(final Point3D point) { return Translation.newBuilder().setX(point.getX()).setY(point.getY()).setZ(point.getZ()).build(); }
final AxisAlignedBoundingBox3DFloat.Builder builder = AxisAlignedBoundingBox3DFloat.newBuilder(); builder.setDepth((float) (maxY - minY)); builder.setHeight((float) (maxZ - minZ)); builder.setWidth((float) (maxX - minX)); builder.setLeftFrontBottom(Translation.newBuilder().setX(minX).setY(minY).setZ(minZ)); return builder.build();
/** * Gets the rotation of the unit relative to the root location as a Rotation object. * * @param unitConfig the unit config to refer the unit. * @return rotation relative to the root location * @throws NotAvailableException is thrown if the transformation is not available. */ default RotationType.Rotation getUnitRotationGlobal(final UnitConfig unitConfig) throws NotAvailableException { try { final Quat4d quat = getUnitRotationGlobalQuat4d(unitConfig); return RotationType.Rotation.newBuilder().setQw(quat.w).setQx(quat.x).setQy(quat.y).setQz(quat.z).build(); } catch (final NotAvailableException ex) { throw new NotAvailableException("GlobalRotation", ex); } }
@Override public void processData(String id, IdentifiableMessage<String, UnitConfig, UnitConfig.Builder> entry, ProtoBufMessageMap<String, UnitConfig, UnitConfig.Builder> entryMap, ProtoBufRegistry<String, UnitConfig, UnitConfig.Builder> registry) throws CouldNotPerformException, EntryModification { UnitConfig.Builder unitConfig = entry.getMessage().toBuilder(); if (unitConfig.getType() == UnitTemplateType.UnitTemplate.UnitType.LOCATION) { return; } if (unitConfig.hasPlacementConfig() && unitConfig.getPlacementConfig().hasPosition()) { final Pose pose = unitConfig.getPlacementConfig().getPosition(); if (pose.getTranslation().getX() == 0 && pose.getTranslation().getY() == 0 && pose.getTranslation().getZ() == 0) { unitConfig.getPlacementConfigBuilder().clearPosition(); throw new EntryModification(entry.setMessage(unitConfig), this); } } } }
public BoundingBox(Transform3D fromUnitCoordinateToRootCoordinateTransform, AxisAlignedBoundingBox3DFloat box) { setDimensions(box.getWidth(), box.getDepth(), box.getHeight()); Transform3D lfb = toTransform(toVector(box.getLeftFrontBottom())); forwardTransform = new Transform3D(fromUnitCoordinateToRootCoordinateTransform); forwardTransform.mul(lfb); setForwardTransform(forwardTransform); calculateCenters(); }
public static Ray3DFloat getRay(final TrackedPosture3DFloat posture, final JointPair jointPair) { final Vec3DFloat startVec = getVec(posture, jointPair.getJoint1()); final Vec3DFloat endVec = getVec(posture, jointPair.getJoint2()); final Vec3DFloat direction = subtract(endVec, startVec); return Ray3DFloat.newBuilder().setOrigin(endVec).setDirection(direction).build(); }
@Override public PointingRay3DFloatDistribution getRays(TrackedPosture3DFloat posture, boolean right, double pointingProbability) { double handHeightAngle = getHandHeightAngle(posture, right, false); Point3D hand = getPoint3D(posture, right ? Joints.HandRight : Joints.HandLeft); Point3D spineShoulder = getPoint3D(posture, Joints.SpineShoulder); Point3D spineHeadDirection = getPoint3D(posture, Joints.Head).subtract(spineShoulder); double factor = 0; final double[] parameters = getParameters(); for (int i = 0; i < parameters.length; i++) { factor += Math.pow(handHeightAngle, parameters.length - i - 1) * parameters[i]; } Point3D start = spineShoulder.add(spineHeadDirection.multiply(factor)); return PointingRay3DFloatDistribution.newBuilder().addRay(PointingRay3DFloat.newBuilder() .setType(PointingType.OTHER) .setRightHandPointing(right) .setCertainty((float) pointingProbability) .setRay(Ray3DFloatType.Ray3DFloat.newBuilder() .setOrigin(toVec3DFloat(hand)) .setDirection(toVec3DFloat(hand.subtract(start).normalize())) ) ).build(); }
public final double probability(Ray3DFloat ray, BoundingBox box) { Point3d origin = toPoint(ray.getOrigin()); Vector3d direction = toVector(ray.getDirection()); //Transform everything to center coordinates of bounding box. Point3d transformedOrigin = box.toCenterCoordinates(origin); Vector3d transformedDirection = box.toCenterCoordinates(direction); double distance = new Vector3d(origin).length(); //TODO: How much is that?! if (distance < 0.05) { return 1; } return distanceProbability(transformedOrigin, transformedDirection, box.getWidth(), box.getDepth(), box.getHeight()); }
public static PlacementConfig getDefaultPlacement(UnitConfig location) { Rotation rotation = Rotation.newBuilder().setQw(1).setQx(0).setQy(0).setQz(0).build(); Translation translation = Translation.newBuilder().setX(0).setY(0).setZ(0).build(); Pose pose = Pose.newBuilder().setRotation(rotation).setTranslation(translation).build(); return PlacementConfig.newBuilder().setPosition(pose).setLocationId(location.getId()).build(); }
/** * Gets the center coordinates of the unit's BoundingBox in the unit coordinate system as a Point3d object. * * @param unitConfig the unit config to refer the unit. * @return center coordinates of the unit's BoundingBox relative to unit * @throws NotAvailableException is thrown if the center can not be calculate. */ default Point3d getUnitBoundingBoxCenterPoint3d(final UnitConfig unitConfig) throws NotAvailableException { final AxisAlignedBoundingBox3DFloatType.AxisAlignedBoundingBox3DFloat bb = getUnitShape(unitConfig).getBoundingBox(); final TranslationType.Translation lfc = bb.getLeftFrontBottom(); final Point3d center = new Point3d(bb.getWidth(), bb.getDepth(), bb.getHeight()); center.scale(0.5); center.add(new Point3d(lfc.getX(), lfc.getY(), lfc.getZ())); return center; }
/** * Gets the position of the unit relative to the root location as a Translation object. * * @param unitConfig the unit config to refer the unit. * @return position relative to the root location * @throws NotAvailableException is thrown if the transformation is not available. */ default TranslationType.Translation getUnitPositionGlobal(final UnitConfig unitConfig) throws NotAvailableException { try { final Point3d pos = getUnitPositionGlobalPoint3d(unitConfig); return TranslationType.Translation.newBuilder().setX(pos.x).setY(pos.y).setZ(pos.z).build(); } catch (NotAvailableException ex) { throw new NotAvailableException("GlobalPosition", ex); } }