Commit 6a1c1e16 authored by Tyler Wilson's avatar Tyler Wilson Committed by Trent Hare
Browse files

Added framing test for setFocalPlane. (#89)

* Added framing test for setFocalPlane.  Also enabled the testing of protected member functions, and modification of the ISD within the test fixture.

* Added jupyter_notebooks folder to CSM-CameraModel.

* Refactored setFocalPlane test so that the ISD changes are made local to the test (and not global).

* Refactored tests for setFocalPlane, distortionJacobian to modify Isd parameters and use them in tests.

* Added two new failing tests and a printIsd() helper function in the FrameIsdTest fixture to help with debugging.

* Fixed the failing tests.  They were due to the Jacobian being unexpectedly singular.
parent b0cb20e3
Loading
Loading
Loading
Loading
+7 −3
Original line number Diff line number Diff line
@@ -5,7 +5,7 @@
#include <cmath>
#include <iostream>
#include <vector>

#include <gtest/gtest.h>
#include "RasterGM.h"
#include "CorrelationModel.h"

@@ -298,8 +298,11 @@ class UsgsAstroFrameSensorModel : public csm::RasterGM {

    static const std::string _SENSOR_MODEL_NAME;


protected:
    FRIEND_TEST(FrameIsdTest, setFocalPlane1);
    FRIEND_TEST(FrameIsdTest, Jacobian1);
    FRIEND_TEST(FrameIsdTest, setFocalPlane_AllCoefficientsOne);
    FRIEND_TEST(FrameIsdTest, distortMe_AllCoefficientsOne);

    virtual bool setFocalPlane(double dx,double dy,double &undistortedX,double &undistortedY) const;
    virtual void distortionFunction(double ux, double uy, double &dx, double &dy) const;
@@ -307,6 +310,7 @@ class UsgsAstroFrameSensorModel : public csm::RasterGM {
                                    double &Jxy, double &Jyx, double &Jyy) const;



  private:

    // Input parameters
+474 −0
Original line number Diff line number Diff line
%% Cell type:markdown id: tags:

# Image to ground

## by Kaj Williams and Jesse Mapel
## Notes: includes rotation.

%% Cell type:code id: tags:

``` python
import numpy as np
from __future__ import print_function, division
```

%% Cell type:code id: tags:

``` python
Samples = 7.5
Lines = 7.5

# optical center (pixels) in x,y direction
Cx=7.5
Cy=7.5

# focal length (m)
F=50.0e-3

# size of pixels in world units (m)
Px=1.0e-4
Py=1.0e-4

# observer position:
obs_x=1000.0
obs_y=0.0
obs_z=0.0

# radius of body (m):
major_radius=10.0
minor_radius=10.0

#Rotation:
omega=0
phi=-np.pi/2.
kappa=np.pi
```

%% Cell type:markdown id: tags:

## Calculation of camera look vector:
$\begin{bmatrix}\mathbf{x} & \mathbf{y} & \mathbf{z} \end{bmatrix}=
\begin{bmatrix}\mathbf{Samples} & \mathbf{Lines} & \mathbf{1} \end{bmatrix}
\begin{bmatrix}
\mathbf{P_y} & \mathbf{0} & \mathbf{0}\\
\mathbf{0} & \mathbf{P_x} & \mathbf{0}\\
\mathbf{-C_y P_y} & \mathbf{-C_x P_x} & \mathbf{F}
\end{bmatrix}
\begin{bmatrix}
\mathbf{sin(\phi)} & \mathbf{-sin(\omega)cos(\phi)} & \mathbf{cos(\omega)cos(\phi)}\\
\mathbf{-cos(\phi)sin(\kappa)} & \mathbf{cos(\omega)cos(\kappa)-sin(\omega)sin(\phi)sin(\kappa)} & \mathbf{cos(\omega)cos(\kappa)+sin(\omega)sin(\phi)sin(\kappa)}\\
\mathbf{cos(\phi)cos(\omega)} & \mathbf{cos(\omega)sin(\kappa)+sin(\omega)sin(\phi)cos(\kappa)} & \mathbf{cos(\omega)sin(\kappa)-sin(\omega)sin(\phi)cos(\kappa)}
\end{bmatrix}
$

%% Cell type:code id: tags:

``` python
image_vector = np.array([Samples, Lines, 1], dtype=float)
```

%% Cell type:code id: tags:

``` python
camera_array = np.array([[Py, 0, 0],[0, Px,0],[-Cy*Py,-Cx*Px,F]], dtype=float)
```

%% Cell type:code id: tags:

``` python
#Compute the camera look vector and normalize it:
camera_look_vector = np.matmul(np.transpose(image_vector),camera_array)
camera_look_vector=camera_look_vector/np.linalg.norm(camera_look_vector,2)
```

%% Cell type:code id: tags:

``` python
rotation_matrix=np.ndarray(shape=(3,3), dtype=float, order='F')
```

%% Cell type:code id: tags:

``` python
rotation_matrix[0,0]=np.cos(phi)*np.cos(kappa)
rotation_matrix[1,0]=np.cos(omega)*np.sin(kappa)+np.sin(omega)*np.sin(phi)*np.cos(kappa)
rotation_matrix[2,0]=np.sin(omega)*np.sin(kappa)-np.cos(omega)*np.sin(phi)*np.cos(kappa)
rotation_matrix[0,1]=-np.cos(phi)*np.sin(kappa)
rotation_matrix[1,1]=np.cos(omega)*np.cos(kappa)-np.sin(omega)*np.sin(phi)*np.sin(kappa)
rotation_matrix[2,1]=np.sin(omega)*np.cos(kappa)+np.cos(omega)*np.sin(phi)*np.sin(kappa)
rotation_matrix[0,2]=np.sin(phi)
rotation_matrix[1,2]=-np.sin(omega)*np.cos(phi)
rotation_matrix[2,2]=np.cos(omega)*np.cos(phi)
```

%% Cell type:code id: tags:

``` python
print(rotation_matrix)
```

%% Output

    [[-6.12323400e-17 -7.49879891e-33 -1.00000000e+00]
     [ 1.22464680e-16 -1.00000000e+00 -0.00000000e+00]
     [-1.00000000e+00 -1.22464680e-16  6.12323400e-17]]

%% Cell type:code id: tags:

``` python
look_vector=np.matmul(np.transpose(camera_look_vector),rotation_matrix)
```

%% Cell type:code id: tags:

``` python
print("After accounting for rotation,  look vector x,y,z: ",look_vector)
```

%% Output

    After accounting for rotation,  look vector x,y,z:  [-1.0000000e+00 -1.2246468e-16  6.1232340e-17]

%% Cell type:code id: tags:

``` python
radius_squared_ratio =major_radius**2/minor_radius**2
a=look_vector[0]**2 + look_vector[1]**2 + radius_squared_ratio*look_vector[2]**2
b=2*(look_vector[0]*obs_x+look_vector[1]*obs_y+radius_squared_ratio*look_vector[2]*obs_z)
c=obs_x**2+obs_y**2+radius_squared_ratio*obs_z**2-major_radius**2
discriminant=b**2-4.0*a*c
print('a:{} b:{} c:{} radius_squared_ratio:{}'.format(a,b,c,radius_squared_ratio))
```

%% Output

    a:1.0 b:-2000.0 c:999900.0 radius_squared_ratio:1.0

%% Cell type:code id: tags:

``` python
if discriminant<0 :
    discriminant=0 # closest intersection
discriminant
```

%% Output

    400.0

%% Cell type:code id: tags:

``` python
distance=(-b-np.sqrt(discriminant))/(2*a)
distance
```

%% Output

    990.0

%% Cell type:code id: tags:

``` python
obs_vector=np.array([obs_x, obs_y, obs_z])
```

%% Cell type:code id: tags:

``` python
ground_point = obs_vector+distance*look_vector
```

%% Cell type:code id: tags:

``` python
print("Planet coords. x,y,z: ",ground_point)
```

%% Output

    Planet coords. x,y,z:  [ 1.00000000e+01 -1.21240033e-13  6.06200166e-14]

%% Cell type:markdown id: tags:

# Ground to Image
### Notes: very basic implementation.

%% Cell type:code id: tags:

``` python
# Look vector:
x=0
y=0
z=-1
```

%% Cell type:markdown id: tags:

$\begin{bmatrix}\mathbf{Samples} & \mathbf{Lines} & \mathbf{1} \end{bmatrix}=
\begin{bmatrix}\mathbf{x} & \mathbf{y} & \mathbf{z}& \mathbf{1} \end{bmatrix}
\begin{bmatrix}
\mathbf{0} & \mathbf{1/P_x} & \mathbf{0}\\
\mathbf{1/P_y} & \mathbf{0} & \mathbf{0}\\
\mathbf{0} & \mathbf{0} & \mathbf{0}\\
\mathbf{C_y} & \mathbf{C_x} & \mathbf{1}
\end{bmatrix}
$

%% Cell type:code id: tags:

``` python
ground_vector=np.array([x, y, z,1], dtype=float)
```

%% Cell type:code id: tags:

``` python
camera_array = np.array([[0, 1/Px, 0],[1/Py, 0,0],[0,0,0],[Cy,Cx,1]], dtype=float)
```

%% Cell type:code id: tags:

``` python
Image_coords = np.matmul(np.transpose(ground_vector),camera_array)
```

%% Cell type:code id: tags:

``` python
print("Image coords. x,y,z: ",Image_coords)
```

%% Output

    Image coords. x,y,z:  [7.5 7.5 1. ]

%% Cell type:markdown id: tags:

# Radial Distortion Model

### The following code is for the Brown-Conrady model, implemented as the "division model" approach:
### https://en.wikipedia.org/wiki/Distortion_(optics)

%% Cell type:code id: tags:

``` python
#Radial distortion coefficients:
# if K1>0 then pincushion distortion.
# If K1<0 then barrel distortion.
K1=0.1
K2=0.0

# Distortion center x,y:
Xc=7.5
Yc=7.5

# Distorted coords x,y:
Xd=8.0
Yd=8.0

```

%% Cell type:code id: tags:

``` python
#helper expression for r^2:
r2=(Xd-Xc)*(Xd-Xc)+(Yd-Yc)*(Yd-Yc)
```

%% Cell type:markdown id: tags:

$X_u=X_c+\frac{(X_d-X_c)}{(1+K_1 r^2+K_2 r^4)}$

%% Cell type:markdown id: tags:

$Y_u=Y_c+\frac{(Y_d-Y_c)}{(1+K_1 r^2+K_2 r^4)}$

%% Cell type:code id: tags:

``` python
X_undistorted = Xc+(Xd-Xc)/(1+K1*r2+K2*r2*r2)
```

%% Cell type:code id: tags:

``` python
Y_undistorted = Yc+(Yd-Yc)/(1+K1*r2+K2*r2*r2)
```

%% Cell type:code id: tags:

``` python
print("Undistorted X,Y: ",X_undistorted,Y_undistorted)
```

%% Output

    Undistorted X,Y:  7.976190476190476 7.976190476190476

%% Cell type:markdown id: tags:

# Radial Distortion Model - MDIS NAC
### The following code is from the ik kernel for the MDIS NAC instrument.  The NAC distortion
### was determined by fitting data from a simulation of the NAC's optical behavior to a 3rd order
### Taylor series expansion.


%% Cell type:code id: tags:

``` python
odtx=[1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0]
odty=[0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0]

```

%% Cell type:markdown id: tags:

### [dx,dy]=distortMe(ux,uy):
### Takes in undistorted focal plane coordinates and returns distorted coordinates [dx,dy]

%% Cell type:code id: tags:

``` python
def distortMe(ux,uy,dtx,dty):

    f=[1,ux,uy,ux**2,ux*uy,uy**2,ux**3,uy*ux**2,ux*uy**2,uy**3]
    dx=0.0
    dy=0.0
    for i in range(len(f)):
        dx = dx+f[i]*dtx[i]
        dy = dy+f[i]*dty[i]
    return [dx,dy]

```

%% Cell type:markdown id: tags:

### [ux,uy]=undistortMe(dx,dy):
### Computes undistored focal plane (ux,uy) coordinates given distorted focal plane
### coordinates using the Newton-Raphson Method for root finding a system of equations:

%% Cell type:markdown id: tags:

$$f_1(x_1,x_2,\cdots,x_n) = f_1(\textbf{x}) = \textbf{0}$$
$$f_2(x_1,x_2,\cdots,x_n) = f_2(\textbf{x}) = \textbf{0}$$
$$\vdots$$
$$f_n(x_1,x_2,\cdots,x_n) = f_n(\textbf{x}) = \textbf{0}$$

%% Cell type:markdown id: tags:

### To solve consider Taylor series expansion of f about x to first order:

$$f(\textbf{x} +\delta \textbf{x} ) = f_i(\textbf{x}) + \sum_{j=0}^n\frac{\partial f_i}{\partial x_j}\delta x_j +O(\delta x ^2),i = 1,\cdots,n$$

%% Cell type:markdown id: tags:

### Ignore the $O(\delta x^2)$ terms and let $f(x+\delta x) = 0 $ (ie, $x+\delta x$ is the root we are seeking).  Then:
### $$-f_i(\textbf{x}) = \sum_{j=0}^n\frac{\partial f_i}{\partial x_j}\delta x_j, j=0,\cdots ,n$$

%% Cell type:markdown id: tags:

### Therefore:  $\delta x_j = -f(\textbf{x})J_f^{-1}(\textbf(x)$ ($J$ = the Jacobian of $f$)

%% Cell type:markdown id: tags:

### And finally:
$$ \textbf{x} \Leftarrow \textbf{x}+ \delta(\textbf{x}) = \textbf{x} - J_f^{-1}(\textbf{x})f(\textbf{x})$$

%% Cell type:markdown id: tags:

### In code:


%% Cell type:code id: tags:

``` python
def Jacobian(x,y,odtx,odty):
    d_dx=[0.0,1.0,0.0,2*x,y,0.0,3*x*x,2*x*y,y*y,0.0]
    d_dy=[0.0,0.0,1.0,0.0,x,2*y,0.0,x**2,2*x*y,3*y**2]

    Jxx=0.0;
    Jxy = 0.0;
    Jyx= 0.0;
    Jyy = 0.0;

    for i in range(len(d_dx)):
        Jxx = Jxx+d_dx[i]*odtx[i]
        Jxy = Jxy+d_dy[i]*odtx[i]
        Jyx = Jyx + d_dx[i]*odty[i]
        Jyy = Jyy + d_dy[i]*odty[i]

    return [Jxx,Jxy,Jyx,Jyy]


```

%% Cell type:code id: tags:

``` python
def undistortMe(dx,dy,odtx,odty):
    epsilon =1.4e-5
    maxIts = 60
    #initial guess
    x = dx
    y = dy
    [fx,fy]= distortMe(x,y,odtx,odty)
    for i in range(maxIts):
        print(i)
        [fx,fy]=distortMe(x,y,odtx,odty)
        fx=dx-fx
        fy = dy-fy
        J = Jacobian(x,y,odtx,odty)
        determinant = J[0]*J[3]-J[1]*J[2]

        if (abs(determinant) < 1e-7):
            print("Jacobian is singular.")
            print(determinant)
            break;
        else:
            x = x+(J[3]*fx-J[1]*fy)/determinant
            y = y+(J[0]*fy-J[2]*fx)/determinant
        if (abs(fx)+abs(fy) <= epsilon):
            return [x,y]
    return [dx,dy]



```

%% Cell type:code id: tags:

``` python
```

%% Cell type:code id: tags:

``` python
distortion = distortMe(7.5,7.5,odtx,odty)
undistorted = undistortMe(distortion[0],distortion[1],odtx,odty)

print(distortion)
print (undistorted)
```

%% Output

    0
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    [908.5, 963.75]
    [7.499999999999999, 7.5]
+1 −1
Original line number Diff line number Diff line
@@ -1076,7 +1076,7 @@ bool UsgsAstroFrameSensorModel::setFocalPlane(double dx,double dy,
    distortionJacobian(x, y, Jxx, Jxy, Jyx, Jyy);

    double determinant = Jxx * Jyy - Jxy * Jyx;
    if (determinant < 1E-6) {
    if (fabs(determinant) < 1E-6) {
      //
      // Near-zero determinant. Add error handling here.
      //
+228 −28
Original line number Diff line number Diff line
@@ -2,12 +2,15 @@
#include "UsgsAstroFrameSensorModel.h"

#include <json.hpp>

#include <iostream>
#include <iomanip>
#include <sstream>
#include <fstream>

#include <map>
#include <vector>
#include <gtest/gtest.h>

using namespace std;
using json = nlohmann::json;

class FrameIsdTest : public ::testing::Test {
@@ -15,6 +18,16 @@ class FrameIsdTest : public ::testing::Test {

      csm::Isd isd;

      void printIsd(csm::Isd &localIsd) {
           multimap<string,string> isdmap= localIsd.parameters();
           for (auto it = isdmap.begin(); it != isdmap.end();++it){

                      cout << it->first << " : " << it->second << endl;
           }


    }

   virtual void SetUp() {
      std::ifstream isdFile("data/simpleFramerISD.json");
      json jsonIsd = json::parse(isdFile);
@@ -36,8 +49,12 @@ class FrameIsdTest : public ::testing::Test {
class FrameSensorModel : public ::testing::Test {
   protected:


      protected :
      UsgsAstroFrameSensorModel *sensorModel;



      void SetUp() override {
         sensorModel = NULL;
         std::ifstream isdFile("data/simpleFramerISD.json");
@@ -54,6 +71,7 @@ class FrameSensorModel : public ::testing::Test {
               isd.addParam(it.key(), jsonValue.dump());
            }
         }

         isdFile.close();

         UsgsAstroFramePlugin frameCameraPlugin;
@@ -72,6 +90,7 @@ class FrameSensorModel : public ::testing::Test {
            delete sensorModel;
            sensorModel = NULL;
         }

      }
};

@@ -124,6 +143,187 @@ TEST_F(FrameSensorModel, OffBody4) {
}



TEST_F(FrameIsdTest, setFocalPlane1) {
  int precision  = 20;
  UsgsAstroFramePlugin frameCameraPlugin;
  std:string odtx_key= "odt_x";
  std::string odty_key="odt_y";

  isd.clearParams(odty_key);
  isd.clearParams(odtx_key);

  csm::ImageCoord imagePt(7.5, 7.5);
  double ux,uy;

  vector<double> odtx{0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
  vector<double> odty{0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};


   for (auto & val: odtx){
      ostringstream strval;
      strval << setprecision(precision) << val;
      isd.addParam("odt_x", strval.str());
   }
   for (auto & val: odty){
      ostringstream strval;
      strval << setprecision(precision) << val;
      isd.addParam("odt_y", strval.str());
   }

   csm::Model *model = frameCameraPlugin.constructModelFromISD(
         isd,
         "USGS_ASTRO_FRAME_SENSOR_MODEL");



   UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);

   sensorModel->setFocalPlane(imagePt.samp, imagePt.line, ux, uy);
   EXPECT_NEAR(imagePt.samp,7.5,1e-8 );
   EXPECT_NEAR(imagePt.line,7.5,1e-8);

}



TEST_F(FrameIsdTest, Jacobian1) {

  int precision  = 20;
  UsgsAstroFramePlugin frameCameraPlugin;
  std:string odtx_key= "odt_x";
  std::string odty_key="odt_y";

  isd.clearParams(odty_key);
  isd.clearParams(odtx_key);

   csm::ImageCoord imagePt(7.5, 7.5);

   vector<double> odtx{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0};
   vector<double> odty{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.0};

   for (auto & val: odtx){
      ostringstream strval;
      strval << setprecision(precision) << val;
      isd.addParam("odt_x", strval.str());
   }
   for (auto & val: odty){
      ostringstream strval;
      strval << setprecision(precision) << val;
      isd.addParam("odt_y", strval.str());
   }

   csm::Model *model = frameCameraPlugin.constructModelFromISD(
         isd,
         "USGS_ASTRO_FRAME_SENSOR_MODEL");

   UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);


   double Jxx,Jxy,Jyx,Jyy;
   sensorModel->distortionJacobian(imagePt.samp, imagePt.line, Jxx, Jxy,Jyx,Jyy);



   EXPECT_NEAR(Jxx,56.25,1e-8 );
   EXPECT_NEAR(Jxy,112.5,1e-8);
   EXPECT_NEAR(Jyx,56.25,1e-8);
   EXPECT_NEAR(Jyy,281.25,1e-8);

}


TEST_F(FrameIsdTest, distortMe_AllCoefficientsOne) {

  int precision  = 20;
  UsgsAstroFramePlugin frameCameraPlugin;
  std:string odtx_key= "odt_x";
  std::string odty_key="odt_y";

  isd.clearParams(odty_key);
  isd.clearParams(odtx_key);

   csm::ImageCoord imagePt(7.5, 7.5);

   vector<double> odtx{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
   vector<double> odty{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};

   for (auto & val: odtx){
      ostringstream strval;
      strval << setprecision(precision) << val;
      isd.addParam("odt_x", strval.str());
   }
   for (auto & val: odty){
      ostringstream strval;
      strval << setprecision(precision) << val;
      isd.addParam("odt_y", strval.str());
   }

   csm::Model *model = frameCameraPlugin.constructModelFromISD(
         isd,
         "USGS_ASTRO_FRAME_SENSOR_MODEL");

   UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);


   printIsd(isd);
   double dx,dy;
   sensorModel->distortionFunction(imagePt.samp, imagePt.line,dx,dy );

   cout << "dx:  " << dx << endl;
   cout << "dy:  " << dy << endl;

   EXPECT_NEAR(dx,1872.25,1e-8 );
   EXPECT_NEAR(dy,1872.25,1e-8);

}

TEST_F(FrameIsdTest, setFocalPlane_AllCoefficientsOne) {

  int precision  = 20;
  UsgsAstroFramePlugin frameCameraPlugin;
  std:string odtx_key= "odt_x";
  std::string odty_key="odt_y";

  isd.clearParams(odty_key);
  isd.clearParams(odtx_key);

   csm::ImageCoord imagePt(1872.25, 1872.25);

   vector<double> odtx{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
   vector<double> odty{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};

   for (auto & val: odtx){
      ostringstream strval;
      strval << setprecision(precision) << val;
      isd.addParam("odt_x", strval.str());
   }
   for (auto & val: odty){
      ostringstream strval;
      strval << setprecision(precision) << val;
      isd.addParam("odt_y", strval.str());
   }

   csm::Model *model = frameCameraPlugin.constructModelFromISD(
         isd,
         "USGS_ASTRO_FRAME_SENSOR_MODEL");

   UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);


   double ux,uy;
   sensorModel->setFocalPlane(imagePt.samp, imagePt.line,ux,uy );


   //The Jacobian is singular, so the setFocalPlane should break out of it's iteration and
   //returns the same distorted coordinates that were passed in.
   EXPECT_NEAR(ux,imagePt.samp,1e-8 );
   EXPECT_NEAR(uy,imagePt.line,1e-8);

}



// Focal Length Tests:
TEST_F(FrameIsdTest, FL500_OffBody4) {
   std::string key = "focal_length";
+1 −11
Original line number Diff line number Diff line
@@ -48,16 +48,6 @@
    "kappa": 3.141592653589793,
    "semi_major_axis":0.01,
    "semi_minor_axis":0.01, 
    "transx": [
        0.0,
        0.1,
        0.0
    ],
    "transy": [
        0.0,
        0.0,
        0.1
    ],
    "x_sensor_origin": 1000,
    "y_sensor_origin": 0,
    "z_sensor_origin": 0,