device.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include <kfusion/internal.hpp>
5 //#include "pointer_shift.cu" // contains primitive needed by all cuda functions dealing with rolling tsdf buffer
6 
9 
10 //__kf_device__
11 //kfusion::device::TsdfVolume::TsdfVolume(elem_type* _data, int3 _dims, float3 _voxel_size, float _trunc_dist, int _max_weight)
12 // : data(_data), dims(_dims), voxel_size(_voxel_size), trunc_dist(_trunc_dist), max_weight(_max_weight) {}
13 
14 //__kf_device__
15 //kfusion::device::TsdfVolume::TsdfVolume(const TsdfVolume& other)
16 // : data(other.data), dims(other.dims), voxel_size(other.voxel_size), trunc_dist(other.trunc_dist), max_weight(other.max_weight) {}
17 
19 { return data + x + y*dims.x + z*dims.y*dims.x; }
20 
22 { return data + x + y*dims.x + z*dims.y*dims.x; }
23 
25 { return data + x + dims.x * y; }
26 
28 { return ptr + dims.x * dims.y; }
29 
31 { return ptr + mult * dims.x * dims.y; }
34 
36 {
37  float2 coo;
38  coo.x = __fmaf_rn(f.x, __fdividef(p.x, p.z), c.x);
39  coo.y = __fmaf_rn(f.y, __fdividef(p.y, p.z), c.y);
40  return coo;
41 }
42 
45 
46 __kf_device__ float3 kfusion::device::Reprojector::operator()(int u, int v, float z) const
47 {
48  float x = z * (u - c.x) * finv.x;
49  float y = z * (v - c.y) * finv.y;
50  return make_float3(x, y, z);
51 }
52 
55 
56 __kf_device__ ushort2 kfusion::device::pack_tsdf (float tsdf, int weight)
57 { return make_ushort2 (__float2half_rn (tsdf), weight); }
58 
59 __kf_device__ float kfusion::device::unpack_tsdf(ushort2 value, int& weight)
60 {
61  weight = value.y;
62  return __half2float (value.x);
63 }
64 __kf_device__ float kfusion::device::unpack_tsdf (ushort2 value) { return __half2float (value.x); }
65 
66 
69 
70 namespace kfusion
71 {
72  namespace device
73  {
74  __kf_device__ Vec3f operator*(const Mat3f& m, const Vec3f& v)
75  { return make_float3(dot(m.data[0], v), dot (m.data[1], v), dot (m.data[2], v)); }
76 
77  __kf_device__ Vec3f operator*(const Aff3f& a, const Vec3f& v) { return a.R * v + a.t; }
78 
79  __kf_device__ Vec3f tr(const float4& v) { return make_float3(v.x, v.y, v.z); }
80 
81  struct plus
82  {
83  __kf_device__ float operator () (float l, float r) const { return l + r; }
84  __kf_device__ double operator () (double l, double r) const { return l + r; }
85  };
86 
87  struct gmem
88  {
89  template<typename T> __kf_device__ static T LdCs(T *ptr);
90  template<typename T> __kf_device__ static void StCs(const T& val, T *ptr);
91  };
92 
93  template<> __kf_device__ ushort2 gmem::LdCs(ushort2* ptr);
94  template<> __kf_device__ void gmem::StCs(const ushort2& val, ushort2* ptr);
95  }
96 }
97 
98 
99 #if defined __CUDA_ARCH__ && __CUDA_ARCH__ >= 200
100 
101  #if defined(_WIN64) || defined(__LP64__)
102  #define _ASM_PTR_ "l"
103  #else
104  #define _ASM_PTR_ "r"
105  #endif
106 
107  template<> __kf_device__ ushort2 kfusion::device::gmem::LdCs(ushort2* ptr)
108  {
109  ushort2 val;
110  asm("ld.global.cs.v2.u16 {%0, %1}, [%2];" : "=h"(reinterpret_cast<ushort&>(val.x)), "=h"(reinterpret_cast<ushort&>(val.y)) : _ASM_PTR_(ptr));
111  return val;
112  }
113 
114  template<> __kf_device__ void kfusion::device::gmem::StCs(const ushort2& val, ushort2* ptr)
115  {
116  short cx = val.x, cy = val.y;
117  asm("st.global.cs.v2.u16 [%0], {%1, %2};" : : _ASM_PTR_(ptr), "h"(reinterpret_cast<ushort&>(cx)), "h"(reinterpret_cast<ushort&>(cy)));
118  }
119  #undef _ASM_PTR_
120 
121 #else
122  template<> __kf_device__ ushort2 kfusion::device::gmem::LdCs(ushort2* ptr) { return *ptr; }
123  template<> __kf_device__ void kfusion::device::gmem::StCs(const ushort2& val, ushort2* ptr) { *ptr = val; }
124 #endif
125 
126 
127 
128 
129 
130 
kfusion::device::Projector::operator()
__kf_device__ float2 operator()(const float3 &p) const
Projector.
Definition: device.hpp:35
temp_utils.hpp
kfusion::device::TsdfVolume::data
elem_type *const data
Definition: internal.hpp:35
internal.hpp
kfusion::device::gmem
Definition: device.hpp:87
kfusion::device::Mat3f::data
float3 data[3]
Definition: internal.hpp:27
p
SharedPointer p
Definition: ConvertShared.hpp:42
kfusion::device::Aff3f::t
Vec3f t
Definition: internal.hpp:28
kfusion::device::TsdfVolume::beg
__kf_device__ elem_type * beg(int x, int y) const
Definition: device.hpp:24
kfusion::device::gmem::StCs
static __kf_device__ void StCs(const T &val, T *ptr)
kfusion::device::Vec3f
float3 Vec3f
Definition: internal.hpp:26
kfusion::device::unpack_tsdf
__kf_device__ float unpack_tsdf(ushort2 value, int &weight)
Definition: device.hpp:59
kfusion::device::TsdfVolume::multzstep
__kf_device__ elem_type * multzstep(int mult, elem_type *const ptr) const
Definition: device.hpp:30
kfusion::device::TsdfVolume::zstep
__kf_device__ elem_type * zstep(elem_type *const ptr) const
Definition: device.hpp:27
kfusion::device::Reprojector::operator()
__kf_device__ float3 operator()(int x, int y, float z) const
Reprojector.
Definition: device.hpp:46
kfusion::device::pack_tsdf
__kf_device__ ushort2 pack_tsdf(float tsdf, int weight)
packing/unpacking tsdf volume element
Definition: device.hpp:56
kfusion::device::TsdfVolume::elem_type
ushort2 elem_type
Definition: internal.hpp:33
kfusion
Utility.
Definition: capture.hpp:8
kfusion::device::dot
__kf_device__ float dot(const float3 &v1, const float3 &v2)
Definition: temp_utils.hpp:27
kfusion::device::Mat3f
Definition: internal.hpp:27
kfusion::device::Aff3f
Definition: internal.hpp:28
kfusion::device::Aff3f::R
Mat3f R
Definition: internal.hpp:28
kfusion::device::TsdfVolume::operator()
__kf_device__ elem_type * operator()(int x, int y, int z)
TsdfVolume.
Definition: device.hpp:18
kfusion::device::TsdfVolume::dims
const int3 dims
Definition: internal.hpp:36
__kf_device__
#define __kf_device__
Definition: kernel_containers.hpp:8
kfusion::device::ushort
unsigned short ushort
Definition: internal.hpp:16
kfusion::device::gmem::LdCs
static __kf_device__ T LdCs(T *ptr)
kfusion::device::plus
Definition: device.hpp:81
kfusion::device::tr
__kf_device__ Vec3f tr(const float4 &v)
Definition: device.hpp:79
kfusion::device::operator*
__kf_device__ Vec3f operator*(const Mat3f &m, const Vec3f &v)
Definition: device.hpp:74


lvr2
Author(s): Thomas Wiemann , Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Wed Mar 2 2022 00:37:23