Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/mesh_filter/sensor_model.h>
00038 #include <stdint.h>
00039 #include <stdexcept>
00040
00041 mesh_filter::SensorModel::~SensorModel ()
00042 {
00043 }
00044
00045 mesh_filter::SensorModel::Parameters::Parameters (unsigned width, unsigned height, float near_clipping_plane_distance, float far_clipping_plane_distance)
00046 : width_ (width)
00047 , height_ (height)
00048 , far_clipping_plane_distance_ (far_clipping_plane_distance)
00049 , near_clipping_plane_distance_ (near_clipping_plane_distance)
00050 {
00051 }
00052
00053 mesh_filter::SensorModel::Parameters::~Parameters ()
00054 {
00055 }
00056
00057 void mesh_filter::SensorModel::Parameters::setImageSize (unsigned width, unsigned height)
00058 {
00059 width_ = width;
00060 height_ = height;
00061 }
00062
00063 void mesh_filter::SensorModel::Parameters::setDepthRange (float near, float far)
00064 {
00065 if (near <= 0)
00066 throw std::runtime_error ("Near clipping plane distance needs to be larger than zero!");
00067
00068 if (far <= near)
00069 throw std::runtime_error ("Far clipping plane distance must be larger than the near clipping plane distance!");
00070
00071 near_clipping_plane_distance_ = near;
00072 far_clipping_plane_distance_ = far;
00073 }
00074
00075 unsigned mesh_filter::SensorModel::Parameters::getWidth () const
00076 {
00077 return width_;
00078 }
00079
00080 unsigned mesh_filter::SensorModel::Parameters::getHeight () const
00081 {
00082 return height_;
00083 }
00084
00085 float mesh_filter::SensorModel::Parameters::getNearClippingPlaneDistance () const
00086 {
00087 return near_clipping_plane_distance_;
00088 }
00089
00090 float mesh_filter::SensorModel::Parameters::getFarClippingPlaneDistance () const
00091 {
00092 return far_clipping_plane_distance_;
00093 }
00094
00095 namespace
00096 {
00097 inline unsigned alignment16 (const void * pointer) { return ((uintptr_t)pointer & 15); }
00098 inline bool isAligned16 (const void* pointer) { return (((uintptr_t)pointer & 15) == 0); }
00099 }
00100
00101 void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth (float* depth) const
00102 {
00103 #if HAVE_SSE_EXTENSIONS
00104 const __m128 mmNear = _mm_set1_ps (near_clipping_plane_distance_);
00105 const __m128 mmFar = _mm_set1_ps (far_clipping_plane_distance_);
00106 const __m128 mmNF = _mm_mul_ps (mmNear, mmFar);
00107 const __m128 mmF_N = _mm_sub_ps (mmFar, mmNear);
00108 static const __m128 mmOnes = _mm_set1_ps (1);
00109 static const __m128 mmZeros = _mm_set1_ps (0);
00110
00111 float* depthEnd = depth + width_ * height_;
00112 if (!isAligned16 (depth))
00113 {
00114
00115 unsigned first = 16 - alignment16 (depth);
00116 unsigned idx;
00117 const float near = near_clipping_plane_distance_;
00118 const float far = far_clipping_plane_distance_;
00119 const float nf = near * far;
00120 const float f_n = far - near;
00121
00122 while (depth < depthEnd && idx++ < first)
00123 if (*depth != 0 && *depth != 1)
00124 *depth = nf / (far - *depth * f_n);
00125 else
00126 *depth = 0;
00127
00128
00129 unsigned last = (width_ * height_ - first) & 15;
00130 float* depth2 = depthEnd - last;
00131 while (depth2 < depthEnd)
00132 if (*depth2 != 0 && *depth2 != 1)
00133 *depth2 = nf / (far - *depth2 * f_n);
00134 else
00135 *depth2 = 0;
00136
00137 depthEnd -= last;
00138 }
00139
00140 const __m128* mmEnd = (__m128*) depthEnd;
00141 __m128* mmDepth = (__m128*) depth;
00142
00143 while (mmDepth < mmEnd)
00144 {
00145 __m128 mask = _mm_and_ps (_mm_cmpneq_ps (*mmDepth, mmOnes), _mm_cmpneq_ps (*mmDepth, mmZeros));
00146 *mmDepth = _mm_mul_ps (*mmDepth, mmF_N);
00147 *mmDepth = _mm_sub_ps (mmFar, *mmDepth);
00148 *mmDepth = _mm_div_ps (mmNF, *mmDepth);
00149 *mmDepth = _mm_and_ps (*mmDepth, mask);
00150 ++mmDepth;
00151 }
00152
00153 #else
00154
00155 const float near = near_clipping_plane_distance_;
00156 const float far = far_clipping_plane_distance_;
00157 const float nf = near * far;
00158 const float f_n = far - near;
00159
00160 const float* depthEnd = depth + width_ * height_;
00161 while (depth < depthEnd)
00162 {
00163 if (*depth != 0 && *depth != 1)
00164 *depth = nf / (far - *depth * f_n);
00165 else
00166 *depth = 0;
00167
00168 ++depth;
00169 }
00170 #endif
00171 }
00172
00173 void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth (float* depth) const
00174 {
00175 #if HAVE_SSE_EXTENSIONS
00176
00177 const __m128 mmNear = _mm_set1_ps (near_clipping_plane_distance_);
00178 const __m128 mmFar = _mm_set1_ps (far_clipping_plane_distance_);
00179 const __m128 mmScale = _mm_sub_ps (mmFar, mmNear);
00180 float *depthEnd = depth + width_ * height_;
00181
00182 if (!isAligned16 (depth))
00183 {
00184
00185 unsigned first = 16 - alignment16 (depth);
00186 unsigned idx;
00187 const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_;
00188 const float offset = near_clipping_plane_distance_;
00189 while (depth < depthEnd && idx++ < first)
00190 if (*depth != 0 && *depth != 1.0)
00191 *depth = *depth * scale + offset;
00192 else
00193 *depth = 0;
00194
00195
00196 unsigned last = (width_ * height_ - first) & 15;
00197 float* depth2 = depthEnd - last;
00198 while (depth2 < depthEnd)
00199 if (*depth2 != 0 && *depth != 1.0)
00200 *depth2 = *depth2 * scale + offset;
00201 else
00202 *depth2 = 0;
00203
00204 depthEnd -= last;
00205 }
00206
00207 const __m128* mmEnd = (__m128*) depthEnd;
00208 __m128* mmDepth = (__m128*) depth;
00209
00210 while (mmDepth < mmEnd)
00211 {
00212 *mmDepth = _mm_mul_ps (*mmDepth, mmScale);
00213 *mmDepth = _mm_add_ps (*mmDepth, mmNear);
00214 *mmDepth = _mm_and_ps (*mmDepth, _mm_and_ps (_mm_cmpneq_ps (*mmDepth, mmNear), _mm_cmpneq_ps (*mmDepth, mmFar)));
00215 ++mmDepth;
00216 }
00217 #else
00218 const float* depthEnd = depth + width_ * height_;
00219 const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_;
00220 const float offset = near_clipping_plane_distance_;
00221 while (depth < depthEnd)
00222 {
00223
00224
00225 if (*depth != 0 && *depth != 1.0)
00226 *depth = *depth * scale + offset;
00227 else
00228 *depth = 0;
00229
00230 ++depth;
00231 }
00232 #endif
00233
00234 }