roll back to modelparsed

android/src/main/java/ai.flow.android/sensor/CameraManager.java

@@ -6,19 +6,14 @@
 import ai.flow.common.utils;
 import ai.flow.definitions.Definitions;
 import ai.flow.modeld.ModelExecutor;
-import ai.flow.modeld.ModelExecutorF3;
 import ai.flow.modeld.messages.MsgFrameData;
 import ai.flow.sensor.SensorInterface;
 import ai.flow.sensor.messages.MsgFrameBuffer;
 
 import android.annotation.SuppressLint;
 import android.content.Context;
-import android.hardware.camera2.CameraCaptureSession;
-import android.hardware.camera2.CameraCharacteristics;
 import android.hardware.camera2.CameraMetadata;
 import android.hardware.camera2.CaptureRequest;
-import android.hardware.camera2.CaptureResult;
-import android.hardware.camera2.TotalCaptureResult;
 import android.hardware.camera2.params.MeteringRectangle;
 import android.hardware.camera2.params.TonemapCurve;
 import android.os.Build;
@@ -32,18 +27,14 @@
 import androidx.camera.camera2.interop.Camera2CameraControl;
 import androidx.camera.camera2.interop.Camera2CameraInfo;
 import androidx.camera.camera2.interop.Camera2Interop;
-import androidx.camera.camera2.interop.CaptureRequestOptions;
 import androidx.camera.camera2.interop.ExperimentalCamera2Interop;
 import androidx.camera.core.*;
-import androidx.camera.core.impl.CameraCaptureResult;
-import androidx.camera.core.impl.utils.ExifData;
 import androidx.camera.lifecycle.ProcessCameraProvider;
 import androidx.core.content.ContextCompat;
 import androidx.fragment.app.Fragment;
 import com.google.common.util.concurrent.ListenableFuture;
 
 import messaging.ZMQPubHandler;
-import org.capnproto.PrimitiveList;
 import org.opencv.core.Core;
 
 import java.nio.ByteBuffer;
@@ -54,13 +45,11 @@
 import java.util.concurrent.ExecutionException;
 import java.util.concurrent.ExecutorService;
 import java.util.concurrent.Executors;
-import java.util.concurrent.ThreadPoolExecutor;
 
 import static ai.flow.android.sensor.Utils.fillYUVBuffer;
 import static ai.flow.common.transformations.Camera.CAMERA_TYPE_ROAD;
 
 import androidx.camera.camera2.Camera2Config;
-import androidx.camera.core.CameraProvider;
 import androidx.camera.core.CameraXConfig;
 
 
@@ -303,8 +292,8 @@ private void bindUseCases(@NonNull ProcessCameraProvider cameraProvider) {
         CameraRequests.setCaptureRequestOption(CaptureRequest.CONTROL_AE_MODE, CaptureRequest.CONTROL_AE_MODE_ON);
         CameraRequests.setCaptureRequestOption(CaptureRequest.COLOR_CORRECTION_MODE, CaptureRequest.COLOR_CORRECTION_MODE_FAST);
         CameraRequests.setCaptureRequestOption(CaptureRequest.CONTROL_AE_TARGET_FPS_RANGE, new Range<>(20, 20));
-//        CameraRequests.setCaptureRequestOption(CaptureRequest.CONTROL_AF_MODE, CaptureRequest.CONTROL_AF_MODE_OFF);
-//        CameraRequests.setCaptureRequestOption(CaptureRequest.LENS_FOCUS_DISTANCE, 0f);
+        CameraRequests.setCaptureRequestOption(CaptureRequest.CONTROL_AF_MODE, CaptureRequest.CONTROL_AF_MODE_OFF);
+        CameraRequests.setCaptureRequestOption(CaptureRequest.LENS_FOCUS_DISTANCE, 0f);
         ImageAnalysis imageAnalysis = builder.build();
         imageAnalysis.setAnalyzer(ContextCompat.getMainExecutor(context), myAnalyzer);
 

android/src/main/java/ai.flow.android/sensor/ModelparsedManager.java

@@ -16,7 +16,7 @@ public ModelparsedManager(Context ctx) {
 
     public void start() {
         System.out.println("Modelparsed was called, starting services!");
-//        ServiceModelparsed.start(ctx);
+        ServiceModelparsed.start(ctx);
         // Context ctx = this.ctx
 //        new java.util.Timer().schedule(
 //            new java.util.TimerTask() {

android/src/main/java/ai.flow.android/sensor/Utils.java

@@ -1,5 +1,7 @@
 package ai.flow.android.sensor;
 
+import android.media.Image;
+
 import androidx.camera.core.ImageProxy;
 
 import java.nio.ByteBuffer;
@@ -22,4 +24,22 @@ public static void fillYUVBuffer(ImageProxy image, ByteBuffer yuvBuffer){
             yuvBuffer.put(vPlane.getBuffer());
         }
     }
+
+    public static void fillYUVBuffer(Image image, ByteBuffer yuvBuffer){
+        yuvBuffer.rewind();
+        Image.Plane yPlane = image.getPlanes()[0];
+        yuvBuffer.put(yPlane.getBuffer());
+
+        // interleaved uv pixels, nv12 or nv21
+        if (image.getPlanes()[1].getPixelStride() == 2) {
+            Image.Plane uvPlane = image.getPlanes()[2];
+            yuvBuffer.put(uvPlane.getBuffer());
+        }
+        else{
+            Image.Plane uPlane = image.getPlanes()[1];
+            Image.Plane vPlane = image.getPlanes()[2];
+            yuvBuffer.put(uPlane.getBuffer());
+            yuvBuffer.put(vPlane.getBuffer());
+        }
+    }
 }

common/java/ai.flow.common/transformations/Camera.java

@@ -9,15 +9,18 @@ public class Camera {
 
     public static final boolean FORCE_TELE_CAM_F3 = false;
 
-    // lots of important stuff calculated from this
-    // current set to LG G8 wide camera
     public static float
-        FocalX = 630f,
-        FocalY = 630f,
-        CenterX = 640f,
-        CenterY = 360f;
+        FocalX = 538f,
+        FocalY = 538f,
+        CenterX = 640f, //632
+        CenterY = 360f; //358
 
-    public static int UseCameraID = 0;
+    // Camera 2
+   // MATRIX: [538.2648047477589, 0, 635.4029785884212;
+//0, 538.3225487046863, 348.6366566852139;
+ //0, 0, 1]
+
+    public static int UseCameraID = 2;
 
     // everything autocalculated below
     public static float actual_cam_focal_length = (FocalX + FocalY) * 0.5f;

selfdrive/modeld/java/ai.flow.modeld/CommonModelF2.java

@@ -24,10 +24,16 @@ public class CommonModelF2 {
     public static final int SIZE_ModelOutputMeta = 352 / 4;
     public static final int SIZE_ModelOutputPose = 48 / 4;
     public static final int SIZE_ModelOutputLinesXY = 1056 / 4;
+
+    public static final int SIZE_ModelOutputWideFromDeviceEuler = 24 / 4;
+    public static final int SIZE_ModelOutputRoadTransform = 48 / 4;
+    public static final int SIZE_LateralAction = 4 / 4;
+
     public static final int SIZE_ModelOutputStopLinePrediction = 68 / 4;
     public static final int SIZE_Total = SIZE_ModelOutputPlans + SIZE_ModelOutputLaneLines + SIZE_ModelOutputRoadEdges +
                                          SIZE_ModelOutputLeads + SIZE_ModelOutputMeta +
                                          SIZE_ModelOutputPose;
+    // + SIZE_ModelOutputWideFromDeviceEuler + SIZE_ModelOutputRoadTransform + SIZE_LateralAction;
     public static final int SIZE_ModelOutputLeadPrediction = 204 / 4;
     public static final int DESIRE_LEN = 8;
     public static final int STOP_LINE_MHP_N = 3;

selfdrive/modeld/java/ai.flow.modeld/ModelExecutorF3.java

@@ -4,6 +4,7 @@
 import ai.flow.common.transformations.Camera;
 import ai.flow.common.utils;
 import ai.flow.definitions.Definitions;
+import ai.flow.modeld.messages.MsgCameraOdometery;
 import ai.flow.modeld.messages.MsgModelRaw;
 import ai.flow.modeld.messages.MsgModelDataV2;
 import messaging.ZMQPubHandler;
@@ -71,7 +72,8 @@ public class ModelExecutorF3 extends ModelExecutor {
     public final ZMQPubHandler ph = new ZMQPubHandler();
     public final ZMQSubHandler sh = new ZMQSubHandler(true);
     public MsgModelRaw msgModelRaw = new MsgModelRaw(CommonModelF3.NET_OUTPUT_SIZE);
-    public MsgModelDataV2 msgModelDataV2 = new MsgModelDataV2();
+//    public MsgModelDataV2 msgModelDataV2 = new MsgModelDataV2();
+//    public MsgCameraOdometery msgCameraOdometery = new MsgCameraOdometery();
     public Definitions.LiveCalibrationData.Reader liveCalib;
 
     public long start, end;
@@ -177,10 +179,16 @@ public void ExecuteModel(Definitions.FrameData.Reader wideData, Definitions.Fram
         // publish outputs
         end = System.currentTimeMillis();
 
-        Parser parser = new Parser();
-        ParsedOutputs parsedOutputs = parser.parser(netOutputs);
-        msgModelDataV2.fill(parsedOutputs, processStartTimestamp, lastFrameID, 0, 0f, end-start, end-start);
-        ph.publishBuffer("modelV2", msgModelDataV2.serialize(true));
+//        Parser parser = new Parser();
+//        ParsedOutputs parsedOutputs = parser.parser(netOutputs);
+//        msgModelDataV2.fill(parsedOutputs, processStartTimestamp, lastFrameID, 0, 0f, end-start, end-start);
+//        msgCameraOdometery.fill(parsedOutputs, processStartTimestamp, lastFrameID);
+//
+//        ph.publishBuffer("modelV2", msgModelDataV2.serialize(true));
+//        ph.publishBuffer("cameraOdometry", msgCameraOdometery.serialize(true));
+
+        msgModelRaw.fill(netOutputs, processStartTimestamp, lastFrameID, 0, 0f, end - start);
+        ph.publishBuffer("modelRaw", msgModelRaw.serialize(true));
 
         // compute runtime stats every 10 runs
         timePerIt += end - processStartTimestamp;
@@ -221,7 +229,8 @@ public void init(){
         navfeaturesNDArr = Nd4j.zeros(navFeaturesTensorShape);
         navinstructNDArr = Nd4j.zeros(navInstructionsTensorShape);
 
-        ph.createPublishers(Arrays.asList("modelV2"));
+//        ph.createPublishers(Arrays.asList("modelV2", "cameraOdometry"));
+        ph.createPublishers(Arrays.asList("modelRaw"));
         sh.createSubscribers(Arrays.asList("pulseDesire", "liveCalibration", "lateralPlan"));
 
         inputShapeMap.put("input_imgs", imgTensorShape);

selfdrive/modeld/java/ai.flow.modeld/ParsedOutputs.java

@@ -29,6 +29,9 @@ public class ParsedOutputs {
     public float[] transStd;
     public float[] rot;
     public float[] rotStd;
+//    public float[] wideFromDeviceEuler;
+//    public float[] roadTransform;
+//    public float[] action;
 
     public ParsedOutputs(){
         this.initLanes();
@@ -86,6 +89,7 @@ public void initCameraOdometery(){
         transStd = new float[3];
         rot = new float[3];
         rotStd = new float[3];
+//        wideFromDeviceEuler = new float[2];
     }
 
     public void initLeads(){

selfdrive/modeld/java/ai.flow.modeld/messages/MsgCameraOdometery.java

@@ -16,6 +16,10 @@ public class MsgCameraOdometery extends MessageBase {
     public PrimitiveList.Float.Builder rotStd;
     public PrimitiveList.Float.Builder trans;
     public PrimitiveList.Float.Builder transStd;
+//    public PrimitiveList.Float.Builder wideFromDeviceEuler;
+//    public PrimitiveList.Float.Builder wideFromDeviceEulerStd;
+//    public PrimitiveList.Float.Builder roadTransformTrans;
+//    public PrimitiveList.Float.Builder roadTransformTransStd;
 
     public MsgCameraOdometery(ByteBuffer rawMessageBuffer) {
         super(rawMessageBuffer);
@@ -39,6 +43,10 @@ private void initFields(){
         rotStd = odometry.initRotStd(3);
         trans = odometry.initTrans(3);
         transStd = odometry.initTransStd(3);
+//        wideFromDeviceEuler = odometry.initWideFromDeviceEuler(3);
+//        wideFromDeviceEulerStd = odometry.initWideFromDeviceEulerStd(3);
+//        roadTransformTrans = odometry.initRoadTransformTrans(3);
+//        roadTransformTransStd = odometry.initRoadTransformTransStd(3);
     }
 
     public void fill(ParsedOutputs parsed, long timestamp, int frameId) {

selfdrive/ui/java/ai.flow.app/CalibrationScreens/CalibrateScreen.java

@@ -203,6 +203,8 @@ public void render(float delta) {
                 byte[] cameraMatrixBuffer = MatToByte(calibrator.cameraMatrix);
                 byte[] distortionCoefficientsBuffer = MatToByte(calibrator.distortionCoefficients);
 
+                System.out.println("MATRIX: " + calibrator.cameraMatrix.dump());
+
                 params.put(intrinsicParamName, cameraMatrixBuffer);
                 params.put(distortionParamName, distortionCoefficientsBuffer);
 

selfdrive/ui/java/ai.flow.app/Draw.java

@@ -17,8 +17,8 @@ public class Draw {
     public static final float LEAD_Y_SCALE = 10f;
 
     public static Matrix4 cam_intrinsics = new Matrix4(new float[] {
-            630.f,  0.0f, 0.0f, 0.f,
-            0.0f, 630.f, 0.0f, 0.f,
+            538.f,  0.0f, 0.0f, 0.f,
+            0.0f, 538.f, 0.0f, 0.f,
             640.0f, 360.0f, 1.0f, 0.f,
             0.f, 0.f, 0.f, 0.f
     });