46 float far_clipping_plane_distance)
49 , far_clipping_plane_distance_(far_clipping_plane_distance)
50 , near_clipping_plane_distance_(near_clipping_plane_distance)
67 throw std::runtime_error(
"Near clipping plane distance needs to be larger than zero!");
70 throw std::runtime_error(
"Far clipping plane distance must be larger than the near clipping plane distance!");
98 inline unsigned alignment16(
const void* pointer)
100 return ((uintptr_t)pointer & 15);
102 inline bool isAligned16(
const void* pointer)
104 return (((uintptr_t)pointer & 15) == 0);
110 #if HAVE_SSE_EXTENSIONS 113 const __m128 mmNF = _mm_mul_ps(mmNear, mmFar);
114 const __m128 mmF_N = _mm_sub_ps(mmFar, mmNear);
115 static const __m128 mmOnes = _mm_set1_ps(1);
116 static const __m128 mmZeros = _mm_set1_ps(0);
119 if (!isAligned16(depth))
122 unsigned first = 16 - alignment16(depth);
126 const float nf = near * far;
127 const float f_n = far - near;
129 while (depth < depthEnd && idx++ < first)
130 if (*depth != 0 && *depth != 1)
131 *depth = nf / (far - *depth * f_n);
136 unsigned last = (
width_ * height_ - first) & 15;
137 float* depth2 = depthEnd - last;
138 while (depth2 < depthEnd)
139 if (*depth2 != 0 && *depth2 != 1)
140 *depth2 = nf / (far - *depth2 * f_n);
147 const __m128* mmEnd = (__m128*)depthEnd;
148 __m128* mmDepth = (__m128*)depth;
150 while (mmDepth < mmEnd)
152 __m128 mask = _mm_and_ps(_mm_cmpneq_ps(*mmDepth, mmOnes), _mm_cmpneq_ps(*mmDepth, mmZeros));
153 *mmDepth = _mm_mul_ps(*mmDepth, mmF_N);
154 *mmDepth = _mm_sub_ps(mmFar, *mmDepth);
155 *mmDepth = _mm_div_ps(mmNF, *mmDepth);
156 *mmDepth = _mm_and_ps(*mmDepth, mask);
164 const float nf = near * far;
165 const float f_n = far - near;
168 while (depth < depthEnd)
170 if (*depth != 0 && *depth != 1)
171 *depth = nf / (far - *depth * f_n);
182 #if HAVE_SSE_EXTENSIONS 186 const __m128 mmScale = _mm_sub_ps(mmFar, mmNear);
189 if (!isAligned16(depth))
192 unsigned first = 16 - alignment16(depth);
196 while (depth < depthEnd && idx++ < first)
197 if (*depth != 0 && *depth != 1.0)
198 *depth = *depth * scale + offset;
203 unsigned last = (
width_ * height_ - first) & 15;
204 float* depth2 = depthEnd - last;
205 while (depth2 < depthEnd)
206 if (*depth2 != 0 && *depth != 1.0)
207 *depth2 = *depth2 * scale + offset;
214 const __m128* mmEnd = (__m128*)depthEnd;
215 __m128* mmDepth = (__m128*)depth;
217 while (mmDepth < mmEnd)
219 *mmDepth = _mm_mul_ps(*mmDepth, mmScale);
220 *mmDepth = _mm_add_ps(*mmDepth, mmNear);
221 *mmDepth = _mm_and_ps(*mmDepth, _mm_and_ps(_mm_cmpneq_ps(*mmDepth, mmNear), _mm_cmpneq_ps(*mmDepth, mmFar)));
228 while (depth < depthEnd)
232 if (*depth != 0 && *depth != 1.0)
233 *depth = *depth * scale + offset;
virtual ~SensorModel()
virtual destructor
void setDepthRange(float near, float far)
sets the clipping range
Parameters(unsigned width, unsigned height, float near_clipping_plane_distance, float far_clipping_plane_distance)
Constructor taking core parameters that are required for all sensors.
unsigned width_
width of depth maps generated by the sensor
float far_clipping_plane_distance_
distance of far clipping plane
unsigned getHeight() const
returns the height of depth maps
unsigned getWidth() const
returns the width of depth maps
float near_clipping_plane_distance_
distance of near clipping plane
virtual void transformModelDepthToMetricDepth(float *depth) const
transforms depth values from rendered model to metric depth values
virtual void transformFilteredDepthToMetricDepth(float *depth) const
transforms depth values from filtered depth to metric depth values
float getFarClippingPlaneDistance() const
returns the distance to the far clipping plane
float getNearClippingPlaneDistance() const
returns distance to the near clipping plane
void setImageSize(unsigned width, unsigned height)
sets the image size
unsigned height_
height of depth maps generated by the sensor
virtual ~Parameters()
virtual destructor