sensor_model.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Suat Gedikli */
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,
00046                                                  float far_clipping_plane_distance)
00047   : width_(width)
00048   , height_(height)
00049   , far_clipping_plane_distance_(far_clipping_plane_distance)
00050   , near_clipping_plane_distance_(near_clipping_plane_distance)
00051 {
00052 }
00053 
00054 mesh_filter::SensorModel::Parameters::~Parameters()
00055 {
00056 }
00057 
00058 void mesh_filter::SensorModel::Parameters::setImageSize(unsigned width, unsigned height)
00059 {
00060   width_ = width;
00061   height_ = height;
00062 }
00063 
00064 void mesh_filter::SensorModel::Parameters::setDepthRange(float near, float far)
00065 {
00066   if (near <= 0)
00067     throw std::runtime_error("Near clipping plane distance needs to be larger than zero!");
00068 
00069   if (far <= near)
00070     throw std::runtime_error("Far clipping plane distance must be larger than the near clipping plane distance!");
00071 
00072   near_clipping_plane_distance_ = near;
00073   far_clipping_plane_distance_ = far;
00074 }
00075 
00076 unsigned mesh_filter::SensorModel::Parameters::getWidth() const
00077 {
00078   return width_;
00079 }
00080 
00081 unsigned mesh_filter::SensorModel::Parameters::getHeight() const
00082 {
00083   return height_;
00084 }
00085 
00086 float mesh_filter::SensorModel::Parameters::getNearClippingPlaneDistance() const
00087 {
00088   return near_clipping_plane_distance_;
00089 }
00090 
00091 float mesh_filter::SensorModel::Parameters::getFarClippingPlaneDistance() const
00092 {
00093   return far_clipping_plane_distance_;
00094 }
00095 
00096 namespace
00097 {
00098 inline unsigned alignment16(const void* pointer)
00099 {
00100   return ((uintptr_t)pointer & 15);
00101 }
00102 inline bool isAligned16(const void* pointer)
00103 {
00104   return (((uintptr_t)pointer & 15) == 0);
00105 }
00106 }
00107 
00108 void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(float* depth) const
00109 {
00110 #if HAVE_SSE_EXTENSIONS
00111   const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_);
00112   const __m128 mmFar = _mm_set1_ps(far_clipping_plane_distance_);
00113   const __m128 mmNF = _mm_mul_ps(mmNear, mmFar);
00114   const __m128 mmF_N = _mm_sub_ps(mmFar, mmNear);
00115   static const __m128 mmOnes = _mm_set1_ps(1);
00116   static const __m128 mmZeros = _mm_set1_ps(0);
00117 
00118   float* depthEnd = depth + width_ * height_;
00119   if (!isAligned16(depth))
00120   {
00121     // first depth value without SSE until we reach aligned data
00122     unsigned first = 16 - alignment16(depth);
00123     unsigned idx;
00124     const float near = near_clipping_plane_distance_;
00125     const float far = far_clipping_plane_distance_;
00126     const float nf = near * far;
00127     const float f_n = far - near;
00128 
00129     while (depth < depthEnd && idx++ < first)
00130       if (*depth != 0 && *depth != 1)
00131         *depth = nf / (far - *depth * f_n);
00132       else
00133         *depth = 0;
00134 
00135     // rest of unaligned data at the end
00136     unsigned last = (width_ * height_ - first) & 15;
00137     float* depth2 = depthEnd - last;
00138     while (depth2 < depthEnd)
00139       if (*depth2 != 0 && *depth2 != 1)
00140         *depth2 = nf / (far - *depth2 * f_n);
00141       else
00142         *depth2 = 0;
00143 
00144     depthEnd -= last;
00145   }
00146 
00147   const __m128* mmEnd = (__m128*)depthEnd;
00148   __m128* mmDepth = (__m128*)depth;
00149   // rest is aligned
00150   while (mmDepth < mmEnd)
00151   {
00152     __m128 mask = _mm_and_ps(_mm_cmpneq_ps(*mmDepth, mmOnes), _mm_cmpneq_ps(*mmDepth, mmZeros));
00153     *mmDepth = _mm_mul_ps(*mmDepth, mmF_N);
00154     *mmDepth = _mm_sub_ps(mmFar, *mmDepth);
00155     *mmDepth = _mm_div_ps(mmNF, *mmDepth);
00156     *mmDepth = _mm_and_ps(*mmDepth, mask);
00157     ++mmDepth;
00158   }
00159 
00160 #else
00161   // calculate metric depth values from OpenGL normalized depth buffer
00162   const float near = near_clipping_plane_distance_;
00163   const float far = far_clipping_plane_distance_;
00164   const float nf = near * far;
00165   const float f_n = far - near;
00166 
00167   const float* depthEnd = depth + width_ * height_;
00168   while (depth < depthEnd)
00169   {
00170     if (*depth != 0 && *depth != 1)
00171       *depth = nf / (far - *depth * f_n);
00172     else
00173       *depth = 0;
00174 
00175     ++depth;
00176   }
00177 #endif
00178 }
00179 
00180 void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(float* depth) const
00181 {
00182 #if HAVE_SSE_EXTENSIONS
00183   //* SSE version
00184   const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_);
00185   const __m128 mmFar = _mm_set1_ps(far_clipping_plane_distance_);
00186   const __m128 mmScale = _mm_sub_ps(mmFar, mmNear);
00187   float* depthEnd = depth + width_ * height_;
00188 
00189   if (!isAligned16(depth))
00190   {
00191     // first depth value without SSE until we reach aligned data
00192     unsigned first = 16 - alignment16(depth);
00193     unsigned idx;
00194     const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_;
00195     const float offset = near_clipping_plane_distance_;
00196     while (depth < depthEnd && idx++ < first)
00197       if (*depth != 0 && *depth != 1.0)
00198         *depth = *depth * scale + offset;
00199       else
00200         *depth = 0;
00201 
00202     // rest of unaligned data at the end
00203     unsigned last = (width_ * height_ - first) & 15;
00204     float* depth2 = depthEnd - last;
00205     while (depth2 < depthEnd)
00206       if (*depth2 != 0 && *depth != 1.0)
00207         *depth2 = *depth2 * scale + offset;
00208       else
00209         *depth2 = 0;
00210 
00211     depthEnd -= last;
00212   }
00213 
00214   const __m128* mmEnd = (__m128*)depthEnd;
00215   __m128* mmDepth = (__m128*)depth;
00216   // rest is aligned
00217   while (mmDepth < mmEnd)
00218   {
00219     *mmDepth = _mm_mul_ps(*mmDepth, mmScale);
00220     *mmDepth = _mm_add_ps(*mmDepth, mmNear);
00221     *mmDepth = _mm_and_ps(*mmDepth, _mm_and_ps(_mm_cmpneq_ps(*mmDepth, mmNear), _mm_cmpneq_ps(*mmDepth, mmFar)));
00222     ++mmDepth;
00223   }
00224 #else
00225   const float* depthEnd = depth + width_ * height_;
00226   const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_;
00227   const float offset = near_clipping_plane_distance_;
00228   while (depth < depthEnd)
00229   {
00230     // 0 = on near clipping plane -> we used 0 to mark invalid points -> not visible
00231     // points on far clipping plane needs to be removed too
00232     if (*depth != 0 && *depth != 1.0)
00233       *depth = *depth * scale + offset;
00234     else
00235       *depth = 0;
00236 
00237     ++depth;
00238   }
00239 #endif
00240 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Fri Dec 14 2018 03:32:23