compute uncertainty for stereo as well

For stereo, the uncertainty is calculated as sqrt(2*noise^2*max_m) where
max_m is the max eigenvalue of the pseudo-inverse in the triangulation
routine.

src/core/DICe_PostProcessor.cpp

@@ -829,16 +829,26 @@ Uncertainty_Post_Processor::execute(){
   Teuchos::RCP<DICe::MultiField> field1_rcp = mesh_->get_field(DICe::mesh::field_enums::FIELD_1_FS);
   Teuchos::RCP<DICe::MultiField> uncertainty_rcp = mesh_->get_field(DICe::mesh::field_enums::UNCERTAINTY_FS);
   Teuchos::RCP<DICe::MultiField> uncertainty_angle_rcp = mesh_->get_field(DICe::mesh::field_enums::UNCERTAINTY_ANGLE_FS);
+  Teuchos::RCP<DICe::MultiField> noise_rcp = mesh_->get_field(DICe::mesh::field_enums::NOISE_LEVEL_FS);
+  Teuchos::RCP<DICe::MultiField> max_m_rcp = mesh_->get_field(DICe::mesh::field_enums::STEREO_M_MAX_FS);
   TEUCHOS_TEST_FOR_EXCEPTION(uncertainty_rcp==Teuchos::null || uncertainty_angle_rcp==Teuchos::null, std::runtime_error,"");
   for(int_t subset=0;subset<local_num_points_;++subset){
     const scalar_t angle = field1_rcp->local_value(subset);
     const scalar_t sig = sigma_rcp->local_value(subset);
+    uncertainty_angle_rcp->local_value(subset) = field1_rcp->local_value(subset);
     if(sig < 0.0){ // filter failed subsets
       uncertainty_rcp->local_value(subset) = 0.0;
       continue;
     }
-    uncertainty_angle_rcp->local_value(subset) = field1_rcp->local_value(subset);
-    uncertainty_rcp->local_value(subset) = angle == 0.0 ? 0.0 : 1.0 / angle * sig;
+    // relying on the max-m field to be zero for 2D and have a non-zero value for stereo
+    scalar_t max_m  = max_m_rcp->local_value(subset);
+    if(max_m > 0.0){
+      scalar_t noise_level = noise_rcp->local_value(subset);
+      uncertainty_rcp->local_value(subset) = std::sqrt(2.0*noise_level*noise_level/max_m);
+    }
+    else{
+      uncertainty_rcp->local_value(subset) = angle == 0.0 ? 0.0 : 1.0 / angle * sig;
+    }
   }
   DEBUG_MSG("Uncertainty_Post_Processor::execute(): end");
 }

src/core/DICe_Schema.cpp

@@ -1183,6 +1183,7 @@ Schema::create_mesh_fields(){
   mesh_->create_field(mesh::field_enums::FIELD_8_FS);
   mesh_->create_field(mesh::field_enums::FIELD_9_FS);
   mesh_->create_field(mesh::field_enums::FIELD_10_FS);
+  mesh_->create_field(mesh::field_enums::STEREO_M_MAX_FS);
 
   if(use_incremental_formulation_){
     mesh_->create_field(mesh::field_enums::ACCUMULATED_DISP_FS);
@@ -2675,6 +2676,8 @@ Schema::execute_triangulation(Teuchos::RCP<Triangulation> tri,
   Teuchos::RCP<MultiField> match_left = mesh_->get_field(DICe::mesh::field_enums::MATCH_FS);
   Teuchos::RCP<MultiField> sigma_right = right_schema->mesh()->get_field(DICe::mesh::field_enums::SIGMA_FS);
 
+  Teuchos::RCP<MultiField> max_m = mesh_->get_field(DICe::mesh::field_enums::STEREO_M_MAX_FS);
+
 //  // set up a neighbor search tree:
 //  Teuchos::RCP<Point_Cloud<scalar_t> > point_cloud = Teuchos::rcp(new Point_Cloud<scalar_t>());
 //  point_cloud->pts.resize(local_num_subsets_);
@@ -2775,6 +2778,7 @@ Schema::execute_triangulation(Teuchos::RCP<Triangulation> tri,
   scalar_t Xw=0.0,Yw=0.0,Zw=0.0;
   scalar_t xl=0.0,yl=0.0;
   scalar_t xr=0.0,yr=0.0;
+  scalar_t max_m_value = 0.0;
   // if this is the first frame and a best fit plane is being used, clear the transform entries in case they have already been specified by the user
 
   bool best_fit = false;
@@ -2793,7 +2797,8 @@ Schema::execute_triangulation(Teuchos::RCP<Triangulation> tri,
     yl = coords_y->local_value(i) + disp_y->local_value(i);
     xr = stereo_coords_x->local_value(i) + my_stereo_disp_x->local_value(i);
     yr = stereo_coords_y->local_value(i) + my_stereo_disp_y->local_value(i);
-    tri->triangulate(xl,yl,xr,yr,X,Y,Z,Xw,Yw,Zw);
+    max_m_value = tri->triangulate(xl,yl,xr,yr,X,Y,Z,Xw,Yw,Zw);
+    max_m->local_value(i) = max_m_value;
     if(frame_id_==first_frame_id_+1){
       model_x->local_value(i) = Xw; // w-coordinates have been transformed by a user defined transform to world or model coords
       model_y->local_value(i) = Yw;

src/core/DICe_Triangulation.cpp

@@ -704,7 +704,7 @@ Triangulation::cosine_of_two_vectors(const std::vector<scalar_t> & a,
 }
 
 
-void Triangulation::triangulate(const scalar_t & x0,
+scalar_t Triangulation::triangulate(const scalar_t & x0,
   const scalar_t & y0,
   const scalar_t & x1,
   const scalar_t & y1,
@@ -805,6 +805,7 @@ void Triangulation::triangulate(const scalar_t & x0,
       }
     }
   }
+
   // compute the inverse of M^TM
   lapack.GETRF(3,3,MTM.values(),3,IPIV_ptr,&INFO);
   try
@@ -826,6 +827,10 @@ void Triangulation::triangulate(const scalar_t & x0,
     }
   }
 
+  scalar_t max_m = std::abs(M(0,0));
+  if(std::abs(M(1,1)) > max_m) max_m = std::abs(M(1,1));
+  if(std::abs(M(2,2)) > max_m) max_m = std::abs(M(2,2));
+
   // compute the 3d point
   for(int_t i=0;i<3;++i){
     for(int_t j=0;j<4;++j){
@@ -848,6 +853,7 @@ void Triangulation::triangulate(const scalar_t & x0,
   yw_out = XYZ[1];
   zw_out = XYZ[2];
   DEBUG_MSG("Triangulation::triangulate(): world coordinates X " << xw_out << " Y " << yw_out << " Z "  << zw_out);
+  return max_m;
 }
 
 void

src/core/DICe_Triangulation.h

@@ -106,6 +106,7 @@ Triangulation{
   }
 
   /// triangulate the optimal point in 3D.
+  /// returns the the max value of the psuedo matrix
   /// global coordinates are always defined with camera 0 as the origin
   /// unless another transformation is requested by specifying a transformation file
   /// \param x0 sensor x coordinate of the point in camera 0
@@ -119,7 +120,7 @@ Triangulation{
   /// \param yw_out global y position in world coords
   /// \param zw_out global z position in world coords
   /// \param correct_lens_distortion correct for lens distortion
-  void triangulate(const scalar_t & x0,
+  scalar_t triangulate(const scalar_t & x0,
     const scalar_t & y0,
     const scalar_t & x1,
     const scalar_t & y1,

src/mesh/DICe_MeshEnums.cpp

@@ -225,6 +225,7 @@ void create_string_maps()
   field_name_string[field_enums::NLVC_STRAIN_YY_ERROR]                               = "NLVC_STRAIN_YY_ERROR";
   field_name_string[field_enums::UNCERTAINTY]                                        = "UNCERTAINTY";
   field_name_string[field_enums::UNCERTAINTY_ANGLE]                                  = "UNCERTAINTY_ANGLE";
+  field_name_string[field_enums::STEREO_M_MAX]                                       = "STEREO_M_MAX";
 
   for (std::map<field_enums::Field_Type,std::string>::iterator pos = field_type_string.begin(); pos != field_type_string.end(); ++pos){
     string_field_type[pos->second] = pos->first;

src/mesh/DICe_MeshEnums.h

@@ -273,7 +273,8 @@ Field_Name
   NLVC_STRAIN_XY_ERROR,
   NLVC_STRAIN_YY_ERROR,
   UNCERTAINTY,
-  UNCERTAINTY_ANGLE
+  UNCERTAINTY_ANGLE,
+  STEREO_M_MAX
 };
 /// The location that the fields live
 enum
@@ -659,6 +660,8 @@ const Field_Spec VSG_DVDX_FS(field_enums::SCALAR_FIELD_TYPE,field_enums::VSG_DVD
 /// field spec
 const Field_Spec VSG_DVDY_FS(field_enums::SCALAR_FIELD_TYPE,field_enums::VSG_DVDY,field_enums::NODE_RANK,field_enums::NO_FIELD_STATE,true);
 /// field spec
+const Field_Spec STEREO_M_MAX_FS(field_enums::SCALAR_FIELD_TYPE,field_enums::STEREO_M_MAX,field_enums::NODE_RANK,field_enums::NO_FIELD_STATE,true);
+/// field spec
 const Field_Spec UNCERTAINTY_FS(field_enums::SCALAR_FIELD_TYPE,field_enums::UNCERTAINTY,field_enums::NODE_RANK,field_enums::NO_FIELD_STATE,true);
 /// field spec
 const Field_Spec UNCERTAINTY_ANGLE_FS(field_enums::SCALAR_FIELD_TYPE,field_enums::UNCERTAINTY_ANGLE,field_enums::NODE_RANK,field_enums::NO_FIELD_STATE,true);
@@ -706,7 +709,7 @@ const Field_Spec NLVC_STRAIN_XY_ERROR_FS(field_enums::SCALAR_FIELD_TYPE,field_en
 const Field_Spec NLVC_STRAIN_YY_ERROR_FS(field_enums::SCALAR_FIELD_TYPE,field_enums::NLVC_STRAIN_YY_ERROR,field_enums::NODE_RANK,field_enums::NO_FIELD_STATE,true);
 
 /// the number of fields that have been defined (must be set at compile time)
-const int_t num_fields_defined = 110;
+const int_t num_fields_defined = 111;
 
 /// array of all the valid field specs
 const field_enums::Field_Spec fs_spec_vec[num_fields_defined] = {
@@ -801,6 +804,7 @@ const field_enums::Field_Spec fs_spec_vec[num_fields_defined] = {
     ALTITUDE_ABOVE_GROUND_FS,
     UNCERTAINTY_FS,
     UNCERTAINTY_ANGLE_FS,
+    STEREO_M_MAX_FS,
     GROUND_LEVEL_FS,
     NLVC_STRAIN_XX_FS,
     NLVC_STRAIN_YY_FS,