41#include <pcl/pcl_config.h>
51 class PCL_EXPORTS ShiftToDepthConverter
55 ShiftToDepthConverter () : init_(false) {}
60 generateLookupTable ()
63 constexpr std::size_t table_size = 1<<10;
66 lookupTable_.resize(table_size);
69 constexpr std::int16_t nConstShift = 800;
70 constexpr double nParamCoeff = 4.000000;
71 constexpr double dPlanePixelSize = 0.104200;
72 constexpr double nShiftScale = 10.000000;
73 constexpr double dPlaneDsr = 120.000000;
74 constexpr double dPlaneDcl = 7.500000;
76 for (std::size_t i=0; i<table_size; ++i)
79 double dFixedRefX = (
static_cast<double>(i - nConstShift) / nParamCoeff)-0.375;
80 double dMetric = dFixedRefX * dPlanePixelSize;
81 lookupTable_[i] =
static_cast<float>((nShiftScale * ((dMetric * dPlaneDsr / (dPlaneDcl - dMetric)) + dPlaneDsr) ) / 1000.0f);
90 shiftToDepth (std::uint16_t shift_val)
94 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
96 float ret = bad_point;
99 if (shift_val<lookupTable_.size())
100 ret = lookupTable_[shift_val];
105 inline bool isInitialized()
const
111 std::vector<float> lookupTable_;