sensor_model.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Suat Gedikli */
36 
38 #include <stdint.h>
39 #include <stdexcept>
40 
42 {
43 }
44 
45 mesh_filter::SensorModel::Parameters::Parameters(unsigned width, unsigned height, float near_clipping_plane_distance,
46  float far_clipping_plane_distance)
47  : width_(width)
48  , height_(height)
49  , far_clipping_plane_distance_(far_clipping_plane_distance)
50  , near_clipping_plane_distance_(near_clipping_plane_distance)
51 {
52 }
53 
55 {
56 }
57 
58 void mesh_filter::SensorModel::Parameters::setImageSize(unsigned width, unsigned height)
59 {
60  width_ = width;
61  height_ = height;
62 }
63 
65 {
66  if (near <= 0)
67  throw std::runtime_error("Near clipping plane distance needs to be larger than zero!");
68 
69  if (far <= near)
70  throw std::runtime_error("Far clipping plane distance must be larger than the near clipping plane distance!");
71 
74 }
75 
77 {
78  return width_;
79 }
80 
82 {
83  return height_;
84 }
85 
87 {
89 }
90 
92 {
94 }
95 
96 namespace
97 {
98 inline unsigned alignment16(const void* pointer)
99 {
100  return ((uintptr_t)pointer & 15);
101 }
102 inline bool isAligned16(const void* pointer)
103 {
104  return (((uintptr_t)pointer & 15) == 0);
105 }
106 }
107 
109 {
110 #if HAVE_SSE_EXTENSIONS
111  const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_);
112  const __m128 mmFar = _mm_set1_ps(far_clipping_plane_distance_);
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);
117 
118  float* depthEnd = depth + width_ * height_;
119  if (!isAligned16(depth))
120  {
121  // first depth value without SSE until we reach aligned data
122  unsigned first = 16 - alignment16(depth);
123  unsigned idx;
124  const float near = near_clipping_plane_distance_;
125  const float far = far_clipping_plane_distance_;
126  const float nf = near * far;
127  const float f_n = far - near;
128 
129  while (depth < depthEnd && idx++ < first)
130  if (*depth != 0 && *depth != 1)
131  *depth = nf / (far - *depth * f_n);
132  else
133  *depth = 0;
134 
135  // rest of unaligned data at the end
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);
141  else
142  *depth2 = 0;
143 
144  depthEnd -= last;
145  }
146 
147  const __m128* mmEnd = (__m128*)depthEnd;
148  __m128* mmDepth = (__m128*)depth;
149  // rest is aligned
150  while (mmDepth < mmEnd)
151  {
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);
157  ++mmDepth;
158  }
159 
160 #else
161  // calculate metric depth values from OpenGL normalized depth buffer
162  const float near = near_clipping_plane_distance_;
163  const float far = far_clipping_plane_distance_;
164  const float nf = near * far;
165  const float f_n = far - near;
166 
167  const float* depthEnd = depth + width_ * height_;
168  while (depth < depthEnd)
169  {
170  if (*depth != 0 && *depth != 1)
171  *depth = nf / (far - *depth * f_n);
172  else
173  *depth = 0;
174 
175  ++depth;
176  }
177 #endif
178 }
179 
181 {
182 #if HAVE_SSE_EXTENSIONS
183  //* SSE version
184  const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_);
185  const __m128 mmFar = _mm_set1_ps(far_clipping_plane_distance_);
186  const __m128 mmScale = _mm_sub_ps(mmFar, mmNear);
187  float* depthEnd = depth + width_ * height_;
188 
189  if (!isAligned16(depth))
190  {
191  // first depth value without SSE until we reach aligned data
192  unsigned first = 16 - alignment16(depth);
193  unsigned idx;
195  const float offset = near_clipping_plane_distance_;
196  while (depth < depthEnd && idx++ < first)
197  if (*depth != 0 && *depth != 1.0)
198  *depth = *depth * scale + offset;
199  else
200  *depth = 0;
201 
202  // rest of unaligned data at the end
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;
208  else
209  *depth2 = 0;
210 
211  depthEnd -= last;
212  }
213 
214  const __m128* mmEnd = (__m128*)depthEnd;
215  __m128* mmDepth = (__m128*)depth;
216  // rest is aligned
217  while (mmDepth < mmEnd)
218  {
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)));
222  ++mmDepth;
223  }
224 #else
225  const float* depthEnd = depth + width_ * height_;
227  const float offset = near_clipping_plane_distance_;
228  while (depth < depthEnd)
229  {
230  // 0 = on near clipping plane -> we used 0 to mark invalid points -> not visible
231  // points on far clipping plane needs to be removed too
232  if (*depth != 0 && *depth != 1.0)
233  *depth = *depth * scale + offset;
234  else
235  *depth = 0;
236 
237  ++depth;
238  }
239 #endif
240 }
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
Definition: sensor_model.h:155
float far_clipping_plane_distance_
distance of far clipping plane
Definition: sensor_model.h:161
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
Definition: sensor_model.h:164
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
Definition: sensor_model.h:158
virtual ~Parameters()
virtual destructor


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Sun Oct 18 2020 13:17:23