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 <stdexcept>
39 
41 
42 mesh_filter::SensorModel::Parameters::Parameters(unsigned width, unsigned height, float near_clipping_plane_distance,
43  float far_clipping_plane_distance)
44  : width_(width)
45  , height_(height)
46  , far_clipping_plane_distance_(far_clipping_plane_distance)
47  , near_clipping_plane_distance_(near_clipping_plane_distance)
48 {
49 }
50 
52 
53 void mesh_filter::SensorModel::Parameters::setImageSize(unsigned width, unsigned height)
54 {
55  width_ = width;
56  height_ = height;
57 }
58 
60 {
61  if (near <= 0)
62  throw std::runtime_error("Near clipping plane distance needs to be larger than zero!");
63 
64  if (far <= near)
65  throw std::runtime_error("Far clipping plane distance must be larger than the near clipping plane distance!");
66 
67  near_clipping_plane_distance_ = near;
68  far_clipping_plane_distance_ = far;
69 }
70 
72 {
73  return width_;
74 }
75 
77 {
78  return height_;
79 }
80 
82 {
83  return near_clipping_plane_distance_;
84 }
85 
87 {
88  return far_clipping_plane_distance_;
89 }
90 
91 namespace
92 {
93 #if HAVE_SSE_EXTENSIONS
94 inline unsigned alignment16(const void* pointer)
95 {
96  return ((uintptr_t)pointer & 15);
97 }
98 inline bool isAligned16(const void* pointer)
99 {
100  return (((uintptr_t)pointer & 15) == 0);
101 }
102 #endif
103 } // namespace
104 
106 {
107 #if HAVE_SSE_EXTENSIONS
108  const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_);
109  const __m128 mmFar = _mm_set1_ps(far_clipping_plane_distance_);
110  const __m128 mmNF = _mm_mul_ps(mmNear, mmFar);
111  const __m128 mmF_N = _mm_sub_ps(mmFar, mmNear);
112  static const __m128 mmOnes = _mm_set1_ps(1);
113  static const __m128 mmZeros = _mm_set1_ps(0);
114 
115  float* depthEnd = depth + width_ * height_;
116  if (!isAligned16(depth))
117  {
118  // first depth value without SSE until we reach aligned data
119  unsigned first = 16 - alignment16(depth);
120  unsigned idx;
121  const float near = near_clipping_plane_distance_;
122  const float far = far_clipping_plane_distance_;
123  const float nf = near * far;
124  const float f_n = far - near;
125 
126  while (depth < depthEnd && idx++ < first)
127  if (*depth != 0 && *depth != 1)
128  *depth = nf / (far - *depth * f_n);
129  else
130  *depth = 0;
131 
132  // rest of unaligned data at the end
133  unsigned last = (width_ * height_ - first) & 15;
134  float* depth2 = depthEnd - last;
135  while (depth2 < depthEnd)
136  if (*depth2 != 0 && *depth2 != 1)
137  *depth2 = nf / (far - *depth2 * f_n);
138  else
139  *depth2 = 0;
140 
141  depthEnd -= last;
142  }
143 
144  const __m128* mmEnd = (__m128*)depthEnd;
145  __m128* mmDepth = (__m128*)depth;
146  // rest is aligned
147  while (mmDepth < mmEnd)
148  {
149  __m128 mask = _mm_and_ps(_mm_cmpneq_ps(*mmDepth, mmOnes), _mm_cmpneq_ps(*mmDepth, mmZeros));
150  *mmDepth = _mm_mul_ps(*mmDepth, mmF_N);
151  *mmDepth = _mm_sub_ps(mmFar, *mmDepth);
152  *mmDepth = _mm_div_ps(mmNF, *mmDepth);
153  *mmDepth = _mm_and_ps(*mmDepth, mask);
154  ++mmDepth;
155  }
156 
157 #else
158  // calculate metric depth values from OpenGL normalized depth buffer
159  const float near = near_clipping_plane_distance_;
160  const float far = far_clipping_plane_distance_;
161  const float nf = near * far;
162  const float f_n = far - near;
163 
164  const float* depth_end = depth + width_ * height_;
165  while (depth < depth_end)
166  {
167  if (*depth != 0 && *depth != 1)
168  *depth = nf / (far - *depth * f_n);
169  else
170  *depth = 0;
171 
172  ++depth;
173  }
174 #endif
175 }
176 
178 {
179 #if HAVE_SSE_EXTENSIONS
180  //* SSE version
181  const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_);
182  const __m128 mmFar = _mm_set1_ps(far_clipping_plane_distance_);
183  const __m128 mmScale = _mm_sub_ps(mmFar, mmNear);
184  float* depthEnd = depth + width_ * height_;
185 
186  if (!isAligned16(depth))
187  {
188  // first depth value without SSE until we reach aligned data
189  unsigned first = 16 - alignment16(depth);
190  unsigned idx;
191  const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_;
192  const float offset = near_clipping_plane_distance_;
193  while (depth < depthEnd && idx++ < first)
194  if (*depth != 0 && *depth != 1.0)
195  *depth = *depth * scale + offset;
196  else
197  *depth = 0;
198 
199  // rest of unaligned data at the end
200  unsigned last = (width_ * height_ - first) & 15;
201  float* depth2 = depthEnd - last;
202  while (depth2 < depthEnd)
203  if (*depth2 != 0 && *depth != 1.0)
204  *depth2 = *depth2 * scale + offset;
205  else
206  *depth2 = 0;
207 
208  depthEnd -= last;
209  }
210 
211  const __m128* mmEnd = (__m128*)depthEnd;
212  __m128* mmDepth = (__m128*)depth;
213  // rest is aligned
214  while (mmDepth < mmEnd)
215  {
216  *mmDepth = _mm_mul_ps(*mmDepth, mmScale);
217  *mmDepth = _mm_add_ps(*mmDepth, mmNear);
218  *mmDepth = _mm_and_ps(*mmDepth, _mm_and_ps(_mm_cmpneq_ps(*mmDepth, mmNear), _mm_cmpneq_ps(*mmDepth, mmFar)));
219  ++mmDepth;
220  }
221 #else
222  const float* depth_end = depth + width_ * height_;
223  const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_;
224  const float offset = near_clipping_plane_distance_;
225  while (depth < depth_end)
226  {
227  // 0 = on near clipping plane -> we used 0 to mark invalid points -> not visible
228  // points on far clipping plane needs to be removed too
229  if (*depth != 0 && *depth != 1.0)
230  *depth = *depth * scale + offset;
231  else
232  *depth = 0;
233 
234  ++depth;
235  }
236 #endif
237 }
mesh_filter::SensorModel::~SensorModel
virtual ~SensorModel()
virtual destructor
sensor_model.h
mesh_filter::SensorModel::Parameters::getWidth
unsigned getWidth() const
returns the width of depth maps
Definition: sensor_model.cpp:71
mesh_filter::SensorModel::Parameters::setImageSize
void setImageSize(unsigned width, unsigned height)
sets the image size
Definition: sensor_model.cpp:53
uintptr_t
std::uintptr_t uintptr_t
mesh_filter::SensorModel::Parameters::setDepthRange
void setDepthRange(float near, float far)
sets the clipping range
Definition: sensor_model.cpp:59
mesh_filter::SensorModel::Parameters::getNearClippingPlaneDistance
float getNearClippingPlaneDistance() const
returns distance to the near clipping plane
Definition: sensor_model.cpp:81
mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth
virtual void transformModelDepthToMetricDepth(float *depth) const
transforms depth values from rendered model to metric depth values
Definition: sensor_model.cpp:105
mesh_filter::SensorModel::Parameters::getHeight
unsigned getHeight() const
returns the height of depth maps
Definition: sensor_model.cpp:76
mesh_filter::SensorModel::Parameters::getFarClippingPlaneDistance
float getFarClippingPlaneDistance() const
returns the distance to the far clipping plane
Definition: sensor_model.cpp:86
mesh_filter::SensorModel::Parameters::~Parameters
virtual ~Parameters()
virtual destructor
mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth
virtual void transformFilteredDepthToMetricDepth(float *depth) const
transforms depth values from filtered depth to metric depth values
Definition: sensor_model.cpp:177
mesh_filter::SensorModel::Parameters::Parameters
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.
Definition: sensor_model.cpp:42


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon May 27 2024 02:27:57