Commit 03256ab9 authored by MarkG's avatar MarkG Committed by TIGERs GitLab
Browse files

Resolve "Simplify configuration of different geometries"

Closes #1701

See merge request main/Sumatra!1470

sumatra-commit: 7af38e2473356eb2c0c953918fea9ad51662bf5e
parent 10b75b3c
Pipeline #17248 failed with stage
in 5 minutes and 41 seconds
......@@ -59,7 +59,7 @@ public class LogfileAnalyzerVisionCam extends ACam
if (sslPacket.hasGeometry())
{
final CamGeometry geometry = geometryTranslator.translate(sslPacket.getGeometry());
final CamGeometry geometry = geometryTranslator.fromProtobuf(sslPacket.getGeometry());
notifyNewCameraCalibration(geometry);
}
......
......@@ -342,7 +342,7 @@ public class LogfileVisionCam extends ACam implements Runnable
if (sslPacket.hasGeometry())
{
final CamGeometry geometry = geometryTranslator.translate(sslPacket.getGeometry());
final CamGeometry geometry = geometryTranslator.fromProtobuf(sslPacket.getGeometry());
notifyNewCameraCalibration(geometry);
}
......
......@@ -135,7 +135,7 @@ public class SSLVisionCam extends ACam implements Runnable, IReceiverObserver, I
if (sslPacket.hasGeometry())
{
final CamGeometry geometry = geometryTranslator.translate(sslPacket.getGeometry());
final CamGeometry geometry = geometryTranslator.fromProtobuf(sslPacket.getGeometry());
notifyNewCameraCalibration(geometry);
}
......
......@@ -4,26 +4,279 @@
package edu.tigers.sumatra.cam;
import edu.tigers.sumatra.cam.data.CamCalibration;
import edu.tigers.sumatra.cam.data.CamFieldArc;
import edu.tigers.sumatra.cam.data.CamFieldLine;
import edu.tigers.sumatra.cam.data.CamFieldSize;
import edu.tigers.sumatra.cam.data.CamGeometry;
import edu.tigers.sumatra.cam.proto.MessagesRobocupSslGeometry;
import edu.tigers.sumatra.cam.proto.MessagesRobocupSslGeometry.SSL_FieldCircularArc;
import edu.tigers.sumatra.cam.proto.MessagesRobocupSslGeometry.SSL_FieldLineSegment;
import edu.tigers.sumatra.cam.proto.MessagesRobocupSslGeometry.SSL_FieldShapeType;
import edu.tigers.sumatra.cam.proto.MessagesRobocupSslGeometry.SSL_GeometryCameraCalibration;
import edu.tigers.sumatra.cam.proto.MessagesRobocupSslGeometry.SSL_GeometryData;
import edu.tigers.sumatra.cam.proto.MessagesRobocupSslGeometry.SSL_GeometryFieldSize;
import edu.tigers.sumatra.cam.proto.MessagesRobocupSslGeometry.Vector2f;
import edu.tigers.sumatra.math.circle.Arc;
import edu.tigers.sumatra.math.circle.IArc;
import edu.tigers.sumatra.math.line.ILine;
import edu.tigers.sumatra.math.line.Line;
import edu.tigers.sumatra.math.vector.IVector;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.Vector2;
import edu.tigers.sumatra.math.vector.Vector3;
import lombok.extern.log4j.Log4j2;
import org.apache.commons.math3.complex.Quaternion;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.stream.Collectors;
/**
* Translate geometry data from protobuf message to our format
*/
@Log4j2
public class SSLVisionCamGeometryTranslator
{
public CamGeometry translate(final SSL_GeometryData geometryData)
private static final double DEFAULT_CENTER_CIRCLE_RADIUS = 500.0;
public CamGeometry fromProtobuf(final SSL_GeometryData geometryData)
{
Map<Integer, CamCalibration> calibrations = geometryData.getCalibList().stream()
.map(this::fromProtobuf)
.collect(Collectors.toMap(CamCalibration::getCameraId, c -> c));
return CamGeometry.builder()
.fieldSize(fromProtobuf(geometryData.getField()))
.ballModels(geometryData.getModels())
.cameraCalibrations(calibrations)
.build();
}
public SSL_GeometryData toProtobuf(CamGeometry camGeometry)
{
List<SSL_GeometryCameraCalibration> calibrations = camGeometry.getCameraCalibrations().values().stream()
.map(this::toProtobuf)
.collect(Collectors.toUnmodifiableList());
return SSL_GeometryData.newBuilder()
.setModels(camGeometry.getBallModels())
.setField(toProtobuf(camGeometry.getFieldSize()))
.addAllCalib(calibrations)
.build();
}
private CamFieldSize fromProtobuf(SSL_GeometryFieldSize field)
{
List<CamFieldLine> fieldLines = field.getFieldLinesList().stream().map(this::fromProtobuf)
.collect(Collectors.toUnmodifiableList());
List<CamFieldArc> fieldArcs = field.getFieldArcsList().stream().map(this::fromProtobuf)
.collect(Collectors.toUnmodifiableList());
return CamFieldSize.builder()
.fieldLength(field.getFieldLength())
.fieldWidth(field.getFieldWidth())
.goalWidth(field.getGoalWidth())
.goalDepth(field.getGoalDepth())
.boundaryWidth(field.getBoundaryWidth())
.fieldLines(fieldLines)
.fieldArcs(fieldArcs)
.penaltyAreaDepth(field.hasPenaltyAreaDepth() ?
(double) field.getPenaltyAreaDepth() :
penaltyAreaDepthFromLineSegments(fieldLines))
.penaltyAreaWidth(field.hasPenaltyAreaWidth() ?
(double) field.getPenaltyAreaWidth() :
penaltyAreaWidthFromLineSegments(fieldLines))
.centerCircleRadius(field.hasCenterCircleRadius() ?
(double) field.getCenterCircleRadius() :
centerCircleRadiusFromArcs(fieldArcs))
.lineThickness(field.hasLineThickness() ? field.getLineThickness() : 10)
.goalCenterToPenaltyMark(field.hasGoalCenterToPenaltyMark() ? field.getGoalCenterToPenaltyMark() : 8000)
.goalHeight(field.hasGoalHeight() ? field.getGoalHeight() : 155)
.ballRadius(field.hasBallRadius() ? field.getBallRadius() : 21.5)
.robotRadius(field.hasMaxRobotRadius() ? field.getMaxRobotRadius() : 90)
.build();
}
private SSL_GeometryFieldSize toProtobuf(CamFieldSize field)
{
return SSL_GeometryFieldSize.newBuilder()
.setFieldLength((int) field.getFieldLength())
.setFieldWidth((int) field.getFieldWidth())
.setGoalWidth((int) field.getGoalWidth())
.setGoalDepth((int) field.getGoalDepth())
.setBoundaryWidth((int) field.getBoundaryWidth())
.addAllFieldLines(field.getFieldLines().stream().map(this::toProtobuf).collect(Collectors.toList()))
.addAllFieldArcs(field.getFieldArcs().stream().map(this::toProtobuf).collect(Collectors.toList()))
.setPenaltyAreaDepth((int) field.getPenaltyAreaDepth())
.setPenaltyAreaWidth((int) field.getPenaltyAreaWidth())
.setCenterCircleRadius((int) field.getCenterCircleRadius())
.setLineThickness((int) field.getLineThickness())
.setGoalCenterToPenaltyMark((int) field.getGoalCenterToPenaltyMark())
.setGoalHeight((int) field.getGoalHeight())
.setBallRadius((float) field.getBallRadius())
.setMaxRobotRadius((float) field.getRobotRadius())
.build();
}
private CamFieldLine fromProtobuf(SSL_FieldLineSegment lineSegment)
{
IVector2 p1 = Vector2.fromXY(lineSegment.getP1().getX(), lineSegment.getP1().getY());
IVector2 p2 = Vector2.fromXY(lineSegment.getP2().getX(), lineSegment.getP2().getY());
ILine line = Line.fromPoints(p1, p2);
return new CamFieldLine(lineSegment.getName(), lineSegment.getType(), lineSegment.getThickness(), line);
}
private SSL_FieldLineSegment toProtobuf(CamFieldLine lineSegment)
{
IVector2 p1 = lineSegment.getLine().getStart();
IVector2 p2 = lineSegment.getLine().getEnd();
return SSL_FieldLineSegment.newBuilder()
.setName(lineSegment.getName())
.setThickness((float) lineSegment.getThickness())
.setType(lineSegment.getType())
.setP1(Vector2f.newBuilder()
.setX((float) p1.x())
.setY((float) p1.y())
.build())
.setP2(Vector2f.newBuilder()
.setX((float) p2.x())
.setY((float) p2.y())
.build())
.build();
}
private CamFieldArc fromProtobuf(SSL_FieldCircularArc arc)
{
var center = Vector2.fromXY(arc.getCenter().getX(), arc.getCenter().getY());
var circle = Arc.createArc(center, arc.getRadius(), arc.getA1(), arc.getA2() - arc.getA1());
return new CamFieldArc(arc.getName(), arc.getType(), arc.getThickness(), circle);
}
private SSL_FieldCircularArc toProtobuf(CamFieldArc camFieldArc)
{
IArc arc = camFieldArc.getArc();
return SSL_FieldCircularArc.newBuilder()
.setName(camFieldArc.getName())
.setThickness((float) camFieldArc.getThickness())
.setType(camFieldArc.getType())
.setCenter(Vector2f.newBuilder()
.setX((float) arc.center().x())
.setY((float) arc.center().y())
.build())
.setRadius((float) arc.radius())
.setA1((float) arc.getStartAngle())
.setA2((float) (arc.getStartAngle() + arc.getRotation()))
.build();
}
private CamCalibration fromProtobuf(SSL_GeometryCameraCalibration cc)
{
return CamCalibration.builder()
.cameraId(cc.getCameraId())
.focalLength(cc.getFocalLength())
.principalPoint(Vector2.fromXY(cc.getPrincipalPointX(), cc.getPrincipalPointY()))
.distortion(cc.getDistortion())
.rotationQuaternion(new Quaternion(cc.getQ3(), cc.getQ0(), cc.getQ1(), cc.getQ2()))
.translation(Vector3.fromXYZ(cc.getTx(), cc.getTy(), cc.getTz()))
.cameraPosition(
Vector3.fromXYZ(
cc.getDerivedCameraWorldTx(),
cc.getDerivedCameraWorldTy(),
cc.getDerivedCameraWorldTz()
)
)
.build();
}
private SSL_GeometryCameraCalibration toProtobuf(CamCalibration cc)
{
return SSL_GeometryCameraCalibration.newBuilder()
.setCameraId(cc.getCameraId())
.setFocalLength((float) cc.getFocalLength())
.setPrincipalPointX((float) cc.getPrincipalPoint().x())
.setPrincipalPointY((float) cc.getPrincipalPoint().y())
.setDistortion((float) cc.getDistortion())
.setQ0((float) cc.getRotationQuaternion().getQ3())
.setQ1((float) cc.getRotationQuaternion().getQ0())
.setQ2((float) cc.getRotationQuaternion().getQ1())
.setQ3((float) cc.getRotationQuaternion().getQ2())
.setTx((float) cc.getTranslation().x())
.setTy((float) cc.getTranslation().y())
.setTz((float) cc.getTranslation().z())
.setDerivedCameraWorldTx((float) cc.getCameraPosition().x())
.setDerivedCameraWorldTy((float) cc.getCameraPosition().y())
.setDerivedCameraWorldTz((float) cc.getCameraPosition().z())
.build();
}
private double penaltyAreaWidthFromLineSegments(List<CamFieldLine> lines)
{
Set<SSL_FieldShapeType> penaltyStretchTypes = Set.of(
SSL_FieldShapeType.LeftPenaltyStretch,
SSL_FieldShapeType.RightPenaltyStretch
);
return uniqueLengthFromSegments(lines, penaltyStretchTypes);
}
private double penaltyAreaDepthFromLineSegments(List<CamFieldLine> lines)
{
Set<SSL_FieldShapeType> penaltyStretchTypes = Set.of(
SSL_FieldShapeType.LeftFieldLeftPenaltyStretch,
SSL_FieldShapeType.LeftFieldRightPenaltyStretch,
SSL_FieldShapeType.RightFieldLeftPenaltyStretch,
SSL_FieldShapeType.RightFieldRightPenaltyStretch
);
return uniqueLengthFromSegments(lines, penaltyStretchTypes);
}
private double uniqueLengthFromSegments(List<CamFieldLine> lines, Set<SSL_FieldShapeType> penaltyStretchTypes)
{
Map<Integer, CamCalibration> calibrations = new HashMap<>();
geometryData.getCalibList().forEach(calib -> calibrations.put(calib.getCameraId(), new CamCalibration(calib)));
List<CamFieldLine> penaltyStretches = lines.stream()
.filter(l -> penaltyStretchTypes.contains(l.getType()))
.collect(Collectors.toUnmodifiableList());
CamFieldSize fieldSize = new CamFieldSize(geometryData.getField());
if (penaltyStretches.size() != penaltyStretchTypes.size())
{
log.warn("Expected {} penalty stretches, but found: {}", penaltyStretchTypes.size(), penaltyStretches);
return 0;
}
return new CamGeometry(calibrations, fieldSize, geometryData.getModels());
// calculate length of penalty area front line
List<Double> lengths = penaltyStretches.stream()
.map(CamFieldLine::getLine)
.map(ILine::directionVector)
.map(IVector::getLength2)
.distinct()
.collect(Collectors.toUnmodifiableList());
if (lengths.size() != 1)
{
log.warn("Penalty stretch line lengths are not unique: {}", lengths);
return 0;
}
return lengths.get(0);
}
private double centerCircleRadiusFromArcs(List<CamFieldArc> arcs)
{
return arcs.stream()
.filter(a -> a.getType() == MessagesRobocupSslGeometry.SSL_FieldShapeType.CenterCircle)
.findFirst()
.map(camFieldArc -> camFieldArc.getArc().radius())
.orElse(DEFAULT_CENTER_CIRCLE_RADIUS);
}
}
......@@ -11,6 +11,9 @@ import edu.tigers.sumatra.math.vector.IVector3;
import edu.tigers.sumatra.math.vector.Vector2;
import edu.tigers.sumatra.math.vector.Vector3;
import edu.tigers.sumatra.math.vector.Vector3f;
import lombok.AllArgsConstructor;
import lombok.Builder;
import lombok.Value;
import org.apache.commons.math3.complex.Quaternion;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
......@@ -24,18 +27,19 @@ import java.util.Map;
/**
* Data holder for calibration data coming from SSL-Vision.
*
* @author AndreR
*/
@Value
@Builder
@AllArgsConstructor
public class CamCalibration implements IJsonString
{
private final int cameraId;
private final double focalLength;
private final double distortion;
private final IVector2 principalPoint;
private final Quaternion rotationQuaternion;
private final IVector3 translation;
private final IVector3 cameraPosition;
int cameraId;
double focalLength;
double distortion;
IVector2 principalPoint;
Quaternion rotationQuaternion;
IVector3 translation;
IVector3 cameraPosition;
/**
......@@ -43,13 +47,19 @@ public class CamCalibration implements IJsonString
*
* @param cameraId
* @param focalLength
* @param principalPoint
* @param distortion
* @param principalPoint
* @param q
* @param t
*/
public CamCalibration(final int cameraId, final double focalLength, final IVector2 principalPoint,
final double distortion, final Quaternion q, final IVector3 t)
public CamCalibration(
int cameraId,
double focalLength,
double distortion,
IVector2 principalPoint,
Quaternion q,
IVector3 t
)
{
this.cameraId = cameraId;
this.focalLength = focalLength;
......@@ -104,44 +114,6 @@ public class CamCalibration implements IJsonString
}
@Override
public String toString()
{
final StringBuilder builder = new StringBuilder();
builder.append("SSLCameraCalibration [cameraId=");
builder.append(cameraId);
builder.append(", focalLength=");
builder.append(focalLength);
builder.append(", principalPointX=");
builder.append(principalPoint.x());
builder.append(", principalPointY=");
builder.append(principalPoint.y());
builder.append(", distortion=");
builder.append(distortion);
builder.append(", q0=");
builder.append(rotationQuaternion.getQ1());
builder.append(", q1=");
builder.append(rotationQuaternion.getQ2());
builder.append(", q2=");
builder.append(rotationQuaternion.getQ3());
builder.append(", q3=");
builder.append(rotationQuaternion.getQ0());
builder.append(", tx=");
builder.append(translation.x());
builder.append(", ty=");
builder.append(translation.y());
builder.append(", tz=");
builder.append(translation.z());
builder.append(", derivedCameraWorldTx=");
builder.append(cameraPosition.x());
builder.append(", derivedCameraWorldTy=");
builder.append(cameraPosition.y());
builder.append(", derivedCameraWorldTz=");
builder.append(cameraPosition.z());
return builder.toString();
}
@Override
public JSONObject toJSON()
{
......@@ -166,58 +138,29 @@ public class CamCalibration implements IJsonString
/**
* @return the cameraId
*/
public int getCameraId()
{
return cameraId;
}
/**
* @return the focalLength
*/
public double getFocalLength()
{
return focalLength;
}
/**
* @return the distortion
*/
public double getDistortion()
{
return distortion;
}
/**
* @return the principalPoint
*/
public IVector2 getPrincipalPoint()
{
return principalPoint;
}
/**
* @return the rotationQuaternion
*/
public Quaternion getRotationQuaternion()
{
return rotationQuaternion;
}
/**
* Translation vector to translate from world coordinate system to camera coordinate system.
* Convert to protobuf.
*
* @return the translation
* @return
*/
public IVector3 getTranslation()
public SSL_GeometryCameraCalibration toProto()
{
return translation;
return SSL_GeometryCameraCalibration.newBuilder()
.setCameraId(cameraId)
.setFocalLength((float) focalLength)
.setPrincipalPointX((float) principalPoint.x())
.setPrincipalPointY((float) principalPoint.y())
.setDistortion((float) distortion)
.setQ0((float) rotationQuaternion.getQ0())
.setQ1((float) rotationQuaternion.getQ1())
.setQ2((float) rotationQuaternion.getQ2())
.setQ3((float) rotationQuaternion.getQ3())
.setTx((float) translation.x())
.setTy((float) translation.y())
.setTz((float) translation.z())
.setDerivedCameraWorldTx((float) cameraPosition.x())
.setDerivedCameraWorldTy((float) cameraPosition.y())
.setDerivedCameraWorldTz((float) cameraPosition.z())
.build();
}
......@@ -227,7 +170,7 @@ public class CamCalibration implements IJsonString
*
* @return R
*/
public RealMatrix getRotationMatrix()
private RealMatrix getRotationMatrix()
{
Rotation rotation = new Rotation(rotationQuaternion.getQ0(),
rotationQuaternion.getQ1(), rotationQuaternion.getQ2(), rotationQuaternion.getQ3(), false);
......@@ -242,7 +185,7 @@ public class CamCalibration implements IJsonString
* @param world input vector
* @return R*world+t
*/
public IVector3 transformToCamera(final IVector3 world)
private IVector3 transformToCamera(final IVector3 world)
{
RealMatrix rot = getRotationMatrix();
RealMatrix t = new Array2DRowRealMatrix(translation.toArray());
......@@ -259,7 +202,7 @@ public class CamCalibration implements IJsonString
* @param camera input vector
* @return R'*(camera-t)
*/
public IVector3 transformToWorld(final IVector3 camera)
private IVector3 transformToWorld(final IVector3 camera)
{
RealMatrix rot = getRotationMatrix();
RealMatrix t = new Array2DRowRealMatrix(translation.toArray());
......@@ -270,17 +213,6 @@ public class CamCalibration implements IJsonString
}
/**
* Get the camera position in world coordinates.
*
* @return camera position
*/
public IVector3 getCameraPosition()
{
return cameraPosition;
}
/**
* Apply radial distortion.
*
......@@ -319,7 +251,7 @@ public class CamCalibration implements IJsonString
* @param in distorted input vector
* @return undistorted vector
*/
public IVector2 undistort(final IVector2 in)
private IVector2 undistort(final IVector2 in)
{
double rd = in.getLength();
if (SumatraMath.isZero(rd))
......@@ -335,7 +267,7 @@ public class CamCalibration implements IJsonString
/**
* Transform a pixel location to a projected position on the field.
*
* @param im Image pixel coordinates.
* @param im Image pixel coordinates.
* @param objectHeight Height in [mm] of the detected object.
* @return Projected and undistorted location on the field.
*/
......