41 #include <pcl/pcl_config.h>
55 ShiftToDepthConverter () : init_(false) {}
58 virtual ~ShiftToDepthConverter () {};
63 generateLookupTable ()
66 constexpr std::size_t table_size = 1<<10;
69 lookupTable_.resize(table_size);
72 constexpr std::int16_t nConstShift = 800;
73 constexpr
double nParamCoeff = 4.000000;
74 constexpr
double dPlanePixelSize = 0.104200;
75 constexpr
double nShiftScale = 10.000000;
76 constexpr
double dPlaneDsr = 120.000000;
77 constexpr
double dPlaneDcl = 7.500000;
79 for (std::size_t i=0; i<table_size; ++i)
82 double dFixedRefX = (
static_cast<double>(i - nConstShift) / nParamCoeff)-0.375;
83 double dMetric = dFixedRefX * dPlanePixelSize;
84 lookupTable_[i] =
static_cast<float>((nShiftScale * ((dMetric * dPlaneDsr / (dPlaneDcl - dMetric)) + dPlaneDsr) ) / 1000.0f);
93 shiftToDepth (std::uint16_t shift_val)
97 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
99 float ret = bad_point;
102 if (shift_val<lookupTable_.size())
103 ret = lookupTable_[shift_val];
108 inline bool isInitialized()
const
114 std::vector<float> lookupTable_;