Functions
optimizer.cpp File Reference
#include "optimizer.h"
#include <librealsense2/rsutil.h>
#include <algorithm>
#include <array>
#include "coeffs.h"
#include "cost.h"
#include "uvmap.h"
#include "k-to-dsm.h"
#include "debug.h"
#include "utils.h"
Include dependency graph for optimizer.cpp:

Go to the source code of this file.

Functions

std::pair< double, p_matrixcalc_cost_and_grad (const z_frame_data &z_data, const std::vector< double3 > &new_vertices, const yuy2_frame_data &yuy_data, const calib &cal, const p_matrix &p_mat, data_collect *data=nullptr)
 
static p_matrix calc_gradients (const z_frame_data &z_data, const std::vector< double3 > &new_vertices, const yuy2_frame_data &yuy_data, const std::vector< double2 > &uv, const calib &cal, const p_matrix &p_mat, data_collect *data=nullptr)
 
static p_matrix calc_p_gradients (const z_frame_data &z_data, const std::vector< double3 > &new_vertices, const yuy2_frame_data &yuy_data, std::vector< double > interp_IDT_x, std::vector< double > interp_IDT_y, const calib &cal, const p_matrix &p_mat, const std::vector< double > &rc, const std::vector< double2 > &xy, data_collect *data=nullptr)
 
static std::pair< std::vector< double2 >, std::vector< double > > calc_rc (const z_frame_data &z_data, const std::vector< double3 > &new_vertices, const yuy2_frame_data &yuy_data, const calib &cal, const p_matrix &p_mat)
 
static void deproject_pixel_to_point (double point[3], const struct rs2_intrinsics_double *intrin, const double pixel[2], double depth)
 
void deproject_sub_pixel (std::vector< double3 > &points, const rs2_intrinsics_double &intrin, std::vector< byte > const &valid_edges, const double *x, const double *y, const double *depth, double depth_units)
 
template<class T >
void depth_filter (std::vector< T > &filtered, std::vector< T > const &origin, std::vector< byte > const &valid_edge_by_ir, size_t const width, size_t const height)
 
static std::vector< double > depth_mean (std::vector< double > const &local_x, std::vector< double > const &local_y)
 
static std::vector< double > find_local_values_min (std::vector< double > const &local_values)
 
std::vector< bytefind_valid_depth_edges (std::vector< double > const &grad_in_direction, std::vector< byte > const &is_supressed, std::vector< double > const &values_for_subedges, std::vector< double > const &ir_local_edges, const params &p)
 
static std::vector< double > get_direction_deg (std::vector< double > const &gradient_x, std::vector< double > const &gradient_y)
 
static std::vector< double > get_direction_deg2 (std::vector< double > const &gradient_x, std::vector< double > const &gradient_y)
 
double get_max (double x, double y)
 
double get_min (double x, double y)
 
static std::pair< int, int > get_next_index (direction dir, int i, int j, size_t width, size_t height)
 
static std::pair< int, int > get_prev_index (direction dir, int i, int j, size_t width, size_t height)
 
void grid_xy (std::vector< double > &gridx, std::vector< double > &gridy, size_t width, size_t height)
 
template<class T >
std::vector< double > interpolation (std::vector< T > const &grid_points, std::vector< double > const x[], std::vector< double > const y[], size_t dim, size_t valid_size, size_t valid_width)
 
std::vector< uint8_tis_suppressed (std::vector< double > const &local_edges, size_t valid_size)
 
static void project_point_to_pixel (double pixel[2], const struct rs2_intrinsics_double *intrin, const double point[3])
 
template<class T >
void sample_by_mask (std::vector< T > &filtered, std::vector< T > const &origin, std::vector< byte > const &valid_edge_by_ir, size_t const width, size_t const height)
 
static std::vector< double > sum_gradient_depth (std::vector< double > const &gradient, std::vector< double > const &direction_per_pixel)
 
void write_matlab_camera_params_file (rs2_intrinsics const &_intr_depth, calib const &rgb_calibration, float _depth_units, std::string const &dir, char const *filename)
 
template<typename T >
void write_obj (std::fstream &f, T const &o)
 
void zero_margin (std::vector< double > &gradient, size_t margin, size_t width, size_t height)
 

Function Documentation

std::pair<double, p_matrix> calc_cost_and_grad ( const z_frame_data z_data,
const std::vector< double3 > &  new_vertices,
const yuy2_frame_data yuy_data,
const calib cal,
const p_matrix p_mat,
data_collect data = nullptr 
)

Definition at line 1514 of file optimizer.cpp.

static p_matrix calc_gradients ( const z_frame_data z_data,
const std::vector< double3 > &  new_vertices,
const yuy2_frame_data yuy_data,
const std::vector< double2 > &  uv,
const calib cal,
const p_matrix p_mat,
data_collect data = nullptr 
)
static

Definition at line 1486 of file optimizer.cpp.

static p_matrix calc_p_gradients ( const z_frame_data z_data,
const std::vector< double3 > &  new_vertices,
const yuy2_frame_data yuy_data,
std::vector< double >  interp_IDT_x,
std::vector< double >  interp_IDT_y,
const calib cal,
const p_matrix p_mat,
const std::vector< double > &  rc,
const std::vector< double2 > &  xy,
data_collect data = nullptr 
)
static

Definition at line 1387 of file optimizer.cpp.

static std::pair< std::vector<double2>, std::vector<double> > calc_rc ( const z_frame_data z_data,
const std::vector< double3 > &  new_vertices,
const yuy2_frame_data yuy_data,
const calib cal,
const p_matrix p_mat 
)
static

Definition at line 1431 of file optimizer.cpp.

static void deproject_pixel_to_point ( double  point[3],
const struct rs2_intrinsics_double intrin,
const double  pixel[2],
double  depth 
)
static

Definition at line 1199 of file optimizer.cpp.

void deproject_sub_pixel ( std::vector< double3 > &  points,
const rs2_intrinsics_double intrin,
std::vector< byte > const &  valid_edges,
const double *  x,
const double *  y,
const double *  depth,
double  depth_units 
)

Definition at line 1357 of file optimizer.cpp.

template<class T >
void depth_filter ( std::vector< T > &  filtered,
std::vector< T > const &  origin,
std::vector< byte > const &  valid_edge_by_ir,
size_t const  width,
size_t const  height 
)

Definition at line 276 of file optimizer.cpp.

static std::vector< double > depth_mean ( std::vector< double > const &  local_x,
std::vector< double > const &  local_y 
)
static

Definition at line 360 of file optimizer.cpp.

static std::vector< double > find_local_values_min ( std::vector< double > const &  local_values)
static

Definition at line 435 of file optimizer.cpp.

std::vector< byte > find_valid_depth_edges ( std::vector< double > const &  grad_in_direction,
std::vector< byte > const &  is_supressed,
std::vector< double > const &  values_for_subedges,
std::vector< double > const &  ir_local_edges,
const params p 
)

Definition at line 399 of file optimizer.cpp.

static std::vector< double > get_direction_deg ( std::vector< double > const &  gradient_x,
std::vector< double > const &  gradient_y 
)
static

Definition at line 143 of file optimizer.cpp.

static std::vector< double > get_direction_deg2 ( std::vector< double > const &  gradient_x,
std::vector< double > const &  gradient_y 
)
static

Definition at line 162 of file optimizer.cpp.

double get_max ( double  x,
double  y 
)

Definition at line 1333 of file optimizer.cpp.

double get_min ( double  x,
double  y 
)

Definition at line 1337 of file optimizer.cpp.

static std::pair< int, int > get_next_index ( direction  dir,
int  i,
int  j,
size_t  width,
size_t  height 
)
static

Definition at line 209 of file optimizer.cpp.

static std::pair< int, int > get_prev_index ( direction  dir,
int  i,
int  j,
size_t  width,
size_t  height 
)
static

Definition at line 182 of file optimizer.cpp.

void grid_xy ( std::vector< double > &  gridx,
std::vector< double > &  gridy,
size_t  width,
size_t  height 
)

Definition at line 296 of file optimizer.cpp.

template<class T >
std::vector< double > interpolation ( std::vector< T > const &  grid_points,
std::vector< double > const  x[],
std::vector< double > const  y[],
size_t  dim,
size_t  valid_size,
size_t  valid_width 
)

Definition at line 313 of file optimizer.cpp.

std::vector<uint8_t> is_suppressed ( std::vector< double > const &  local_edges,
size_t  valid_size 
)

Definition at line 343 of file optimizer.cpp.

static void project_point_to_pixel ( double  pixel[2],
const struct rs2_intrinsics_double intrin,
const double  point[3] 
)
static

Definition at line 1210 of file optimizer.cpp.

template<class T >
void sample_by_mask ( std::vector< T > &  filtered,
std::vector< T > const &  origin,
std::vector< byte > const &  valid_edge_by_ir,
size_t const  width,
size_t const  height 
)

Definition at line 258 of file optimizer.cpp.

static std::vector< double > sum_gradient_depth ( std::vector< double > const &  gradient,
std::vector< double > const &  direction_per_pixel 
)
static

Definition at line 379 of file optimizer.cpp.

void write_matlab_camera_params_file ( rs2_intrinsics const &  _intr_depth,
calib const &  rgb_calibration,
float  _depth_units,
std::string const &  dir,
char const *  filename 
)

Definition at line 1652 of file optimizer.cpp.

template<typename T >
void write_obj ( std::fstream &  f,
T const &  o 
)

Definition at line 1647 of file optimizer.cpp.

void zero_margin ( std::vector< double > &  gradient,
size_t  margin,
size_t  width,
size_t  height 
)

Definition at line 237 of file optimizer.cpp.



librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:50:27