public void getScanPoint(int index, Point3f scanPointToPack) { index *= 3; scanPointToPack.setX(scan[index++]); scanPointToPack.setY(scan[index++]); scanPointToPack.setZ(scan[index++]); }
public Point3f[] getDecayingWorldScan() { int numberOfPoints = decayingWorldScan.length/3; Point3f[] points = new Point3f[numberOfPoints]; for(int i = 0; i < numberOfPoints; i++) { Point3f point = new Point3f(); point.setX(decayingWorldScan[3 * i]); point.setY(decayingWorldScan[3 * i + 1]); point.setZ(decayingWorldScan[3 * i + 2]); points[i] = point; } return points; }
public Point3f[] getPointMap() { int numberOfPoints = localizationPointMap.length/3; Point3f[] points = new Point3f[numberOfPoints]; for(int i = 0; i < numberOfPoints; i++) { Point3f point = new Point3f(); point.setX(localizationPointMap[3 * i]); point.setY(localizationPointMap[3 * i + 1]); point.setZ(localizationPointMap[3 * i + 2]); points[i] = point; } return points; }
public Point3f[] getPointCloud() { int numberOfPoints = flatPoints.length/3; Point3f[] points = new Point3f[numberOfPoints]; for(int i = 0; i < numberOfPoints; i++) { Point3f point = new Point3f(); point.setX(flatPoints[3 * i]); point.setY(flatPoints[3 * i + 1]); point.setZ(flatPoints[3 * i + 2]); points[i] = point; } return points; }
public Point3f[] getGroundQuadTreeSupport() { int numberOfPoints = groundQuadTreeSupport.length/3; Point3f[] points = new Point3f[numberOfPoints]; for(int i = 0; i < numberOfPoints; i++) { Point3f point = new Point3f(); point.setX(groundQuadTreeSupport[3 * i]); point.setY(groundQuadTreeSupport[3 * i + 1]); point.setZ(groundQuadTreeSupport[3 * i + 2]); points[i] = point; } return points; }
@Override public void set(LidarScanMessage message) { timestamp = message.robotTimestamp; message.getLidarPose(lidarPose); int index = 0; float[] newPointCloud = message.scan; scan.clear(); while (index < newPointCloud.length) { Point3f point = scan.add(); point.setX(newPointCloud[index++]); point.setY(newPointCloud[index++]); point.setZ(newPointCloud[index++]); } }
float vz = vertex.getZ(); vertex.setX(x0 + rxx * vx + rxy * vy + rxz * vz); vertex.setY(y0 + ryx * vx + ryy * vy + ryz * vz); vertex.setZ(z0 + rzx * vx + rzz * vz);
/** * Multiply a 3x3 matrix by a 3x1 vector. Since result is stored in vector, the matrix must be 3x3. * @param matrix * @param point */ public static void mult(DenseMatrix64F matrix, Point3f point) { if (matrix.numCols != 3 || matrix.numRows != 3) { throw new RuntimeException("Improperly sized matrices."); } double x = point.getX(); double y = point.getY(); double z = point.getZ(); point.setX((float) (matrix.get(0, 0) * x + matrix.get(0, 1) * y + matrix.get(0, 2) * z)); point.setY((float) (matrix.get(1, 0) * x + matrix.get(1, 1) * y + matrix.get(1, 2) * z)); point.setZ((float) (matrix.get(2, 0) * x + matrix.get(2, 1) * y + matrix.get(2, 2) * z)); }
/** * Transform the Point3f point by this transform and place result back in * point. * * @param point */ public final void transform(Point3f point) { float x = (float) (mat00 * point.getX() + mat01 * point.getY() + mat02 * point.getZ() + mat03); float y = (float) (mat10 * point.getX() + mat11 * point.getY() + mat12 * point.getZ() + mat13); point.setZ((float) (mat20 * point.getX() + mat21 * point.getY() + mat22 * point.getZ() + mat23)); point.setX(x); point.setY(y); }
/** * Transform the Point3f pointIn by this transform and place result in * pointOut. * * @param point */ public final void transform(Point3f pointIn, Point3f pointOut) { if (pointIn != pointOut) { pointOut.setX((float) (mat00 * pointIn.getX() + mat01 * pointIn.getY() + mat02 * pointIn.getZ() + mat03)); pointOut.setY((float) (mat10 * pointIn.getX() + mat11 * pointIn.getY() + mat12 * pointIn.getZ() + mat13)); pointOut.setZ((float) (mat20 * pointIn.getX() + mat21 * pointIn.getY() + mat22 * pointIn.getZ() + mat23)); } else { transform(pointIn); } }