Skip to content

Commit

Permalink
voxel filter
Browse files Browse the repository at this point in the history
  • Loading branch information
@neka-nat
neka-nat committed Nov 19, 2019
1 parent 4afbe77 commit 9fd8171
Show file tree
Hide file tree
Showing 2 changed files with 154 additions and 23 deletions.
95 changes: 72 additions & 23 deletions src/cupoc/geometry/pointcloud.cu
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,12 +49,33 @@ struct compute_key_functor {
}
};

struct devided_vector_functor : public thrust::binary_function<const Eigen::Vector3f_u, const int, Eigen::Vector3f_u> {
__host__ __device__
Eigen::Vector3f_u operator()(const Eigen::Vector3f_u& x, const int& y) const {
return x / static_cast<float>(y);
}
};
template<typename OutputIterator, class... Args>
__host__
int CalcAverageByKey(thrust::device_vector<Eigen::Vector3i>& keys,
OutputIterator buf_begins, OutputIterator output_begins) {
const size_t n = keys.size();
thrust::sort_by_key(keys.begin(), keys.end(), buf_begins);

thrust::device_vector<Eigen::Vector3i> keys_out(n);
thrust::device_vector<int> counts(n);
auto end1 = thrust::reduce_by_key(keys.begin(), keys.end(),
thrust::make_constant_iterator(1),
keys_out.begin(), counts.begin());
int n_out = static_cast<int>(end1.second - counts.begin());
counts.resize(n_out);

thrust::equal_to<Eigen::Vector3i> binary_pred;
add_tuple_functor<Args...> add_func;
auto end2 = thrust::reduce_by_key(keys.begin(), keys.end(), buf_begins,
keys_out.begin(), output_begins,
binary_pred, add_func);

devided_tuple_functor<Args...> dv_func;
thrust::transform(output_begins, output_begins + n_out,
counts.begin(), output_begins,
dv_func);
return n_out;
}

}

Expand Down Expand Up @@ -107,27 +128,55 @@ utility::shared_ptr<PointCloud> PointCloud::VoxelDownSample(float voxel_size) co
}

const int n = points_.size();
const bool has_normals = HasNormals();
const bool has_colors = HasColors();
compute_key_functor ck_func(voxel_min_bound, voxel_size);
thrust::device_vector<Eigen::Vector3i> keys(n);
thrust::transform(points_.begin(), points_.end(), keys.begin(), ck_func);

thrust::device_vector<Eigen::Vector3f_u> sorted_points(n);
thrust::copy(points_.begin(), points_.end(), sorted_points.begin());
thrust::sort_by_key(keys.begin(), keys.end(), sorted_points.begin());

thrust::device_vector<Eigen::Vector3i> keys_out(n);
thrust::device_vector<int> counts(n);
auto end1 = thrust::reduce_by_key(keys.begin(), keys.end(), thrust::make_constant_iterator(1),
keys_out.begin(), counts.begin());
int n_out = static_cast<int>(end1.second - counts.begin());
counts.resize(n_out);
output->points_.resize(n_out);
auto end2 = thrust::reduce_by_key(keys.begin(), keys.end(), sorted_points.begin(),
keys_out.begin(), output->points_.begin());

devided_vector_functor dv_func;
thrust::transform(output->points_.begin(), output->points_.end(), counts.begin(),
output->points_.begin(), dv_func);
thrust::device_vector<Eigen::Vector3f_u> sorted_points = points_;
output->points_.resize(n);
if (!has_normals && !has_colors) {
typedef thrust::tuple<thrust::device_vector<Eigen::Vector3f_u>::iterator> IteratorTuple;
typedef thrust::zip_iterator<IteratorTuple> ZipIterator;
auto n_out = CalcAverageByKey<ZipIterator, Eigen::Vector3f_u>(keys,
thrust::make_zip_iterator(thrust::make_tuple(sorted_points.begin())),
thrust::make_zip_iterator(thrust::make_tuple(output->points_.begin())));
output->points_.resize(n_out);
} else if (has_normals && !has_colors) {
thrust::device_vector<Eigen::Vector3f_u> sorted_normals = normals_;
output->normals_.resize(n);
typedef thrust::tuple<thrust::device_vector<Eigen::Vector3f_u>::iterator, thrust::device_vector<Eigen::Vector3f_u>::iterator> IteratorTuple;
typedef thrust::zip_iterator<IteratorTuple> ZipIterator;
auto n_out = CalcAverageByKey<ZipIterator, Eigen::Vector3f_u, Eigen::Vector3f_u>(keys,
thrust::make_zip_iterator(thrust::make_tuple(sorted_points.begin(), sorted_normals.begin())),
thrust::make_zip_iterator(thrust::make_tuple(output->points_.begin(), output->normals_.begin())));
output->points_.resize(n_out);
output->normals_.resize(n_out);
} else if (!has_normals && has_colors) {
thrust::device_vector<Eigen::Vector3f_u> sorted_colors = colors_;
output->colors_.resize(n);
typedef thrust::tuple<thrust::device_vector<Eigen::Vector3f_u>::iterator, thrust::device_vector<Eigen::Vector3f_u>::iterator> IteratorTuple;
typedef thrust::zip_iterator<IteratorTuple> ZipIterator;
auto n_out = CalcAverageByKey<ZipIterator, Eigen::Vector3f_u, Eigen::Vector3f_u>(keys,
thrust::make_zip_iterator(thrust::make_tuple(sorted_points.begin(), sorted_colors.begin())),
thrust::make_zip_iterator(thrust::make_tuple(output->points_.begin(), output->colors_.begin())));
output->points_.resize(n_out);
output->colors_.resize(n_out);
} else {
thrust::device_vector<Eigen::Vector3f_u> sorted_normals = normals_;
thrust::device_vector<Eigen::Vector3f_u> sorted_colors = colors_;
output->normals_.resize(n);
output->colors_.resize(n);
typedef thrust::tuple<thrust::device_vector<Eigen::Vector3f_u>::iterator, thrust::device_vector<Eigen::Vector3f_u>::iterator, thrust::device_vector<Eigen::Vector3f_u>::iterator> IteratorTuple;
typedef thrust::zip_iterator<IteratorTuple> ZipIterator;
auto n_out = CalcAverageByKey<ZipIterator, Eigen::Vector3f_u, Eigen::Vector3f_u, Eigen::Vector3f_u>(keys,
thrust::make_zip_iterator(thrust::make_tuple(sorted_points.begin(), sorted_normals.begin(), sorted_colors.begin())),
thrust::make_zip_iterator(thrust::make_tuple(output->points_.begin(), output->normals_.begin(), output->colors_.begin())));
output->points_.resize(n_out);
output->normals_.resize(n_out);
output->colors_.resize(n_out);
}

utility::LogDebug(
"Pointcloud down sampled from {:d} points to {:d} points.\n",
Expand Down
82 changes: 82 additions & 0 deletions src/cupoc/utility/helper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,4 +15,86 @@ struct equal_to<Eigen::Vector3i> {
}
};

}

namespace cupoc {

template<class... Args>
struct add_tuple_functor : public thrust::binary_function<const thrust::tuple<Args...>, const thrust::tuple<Args...>, thrust::tuple<Args...>> {
__host__ __device__
thrust::tuple<Args...> operator()(const thrust::tuple<Args...>& x, const thrust::tuple<Args...>& y) const;
};

template<class... Args>
struct devided_tuple_functor : public thrust::binary_function<const thrust::tuple<Args...>, const int, thrust::tuple<Args...>> {
__host__ __device__
thrust::tuple<Args...> operator()(const thrust::tuple<Args...>& x, const int& y) const;
};

template<class T1>
struct add_tuple_functor<T1> : public thrust::binary_function<const thrust::tuple<T1>, const thrust::tuple<T1>, thrust::tuple<T1>> {
__host__ __device__
thrust::tuple<T1> operator()(const thrust::tuple<T1>& x, const thrust::tuple<T1>& y) const {
thrust::tuple<T1> ans;
thrust::get<0>(ans) = thrust::get<0>(x) + thrust::get<0>(y);
return ans;
}
};

template<class T1, class T2>
struct add_tuple_functor<T1, T2> : public thrust::binary_function<const thrust::tuple<T1, T2>, const thrust::tuple<T1, T2>, thrust::tuple<T1, T2>> {
__host__ __device__
thrust::tuple<T1, T2> operator()(const thrust::tuple<T1, T2>& x, const thrust::tuple<T1, T2>& y) const {
thrust::tuple<T1, T2> ans;
thrust::get<0>(ans) = thrust::get<0>(x) + thrust::get<0>(y);
thrust::get<1>(ans) = thrust::get<1>(x) + thrust::get<1>(y);
return ans;
}
};

template<class T1, class T2, class T3>
struct add_tuple_functor<T1, T2, T3> : public thrust::binary_function<const thrust::tuple<T1, T2, T3>, const thrust::tuple<T1, T2, T3>, thrust::tuple<T1, T2, T3>> {
__host__ __device__
thrust::tuple<T1, T2, T3> operator()(const thrust::tuple<T1, T2, T3>& x, const thrust::tuple<T1, T2, T3>& y) const {
thrust::tuple<T1, T2, T3> ans;
thrust::get<0>(ans) = thrust::get<0>(x) + thrust::get<0>(y);
thrust::get<1>(ans) = thrust::get<1>(x) + thrust::get<1>(y);
thrust::get<2>(ans) = thrust::get<2>(x) + thrust::get<2>(y);
return ans;
}
};

template<class T1>
struct devided_tuple_functor<T1> : public thrust::binary_function<const thrust::tuple<T1>, const int, thrust::tuple<T1>> {
__host__ __device__
thrust::tuple<T1> operator()(const thrust::tuple<T1>& x, const int& y) const {
thrust::tuple<T1> ans;
thrust::get<0>(ans) = thrust::get<0>(x) / static_cast<float>(y);
return ans;
}
};

template<class T1, class T2>
struct devided_tuple_functor<T1, T2> : public thrust::binary_function<const thrust::tuple<T1, T2>, const int, thrust::tuple<T1, T2>> {
__host__ __device__
thrust::tuple<T1, T2> operator()(const thrust::tuple<T1, T2>& x, const int& y) const {
thrust::tuple<T1, T2> ans;
thrust::get<0>(ans) = thrust::get<0>(x) / static_cast<float>(y);
thrust::get<1>(ans) = thrust::get<1>(x) / static_cast<float>(y);
return ans;
}
};

template<class T1, class T2, class T3>
struct devided_tuple_functor<T1, T2, T3> : public thrust::binary_function<const thrust::tuple<T1, T2, T3>, const int, thrust::tuple<T1, T2, T3>> {
__host__ __device__
thrust::tuple<T1, T2, T3> operator()(const thrust::tuple<T1, T2, T3>& x, const int& y) const {
thrust::tuple<T1, T2, T3> ans;
thrust::get<0>(ans) = thrust::get<0>(x) / static_cast<float>(y);
thrust::get<1>(ans) = thrust::get<1>(x) / static_cast<float>(y);
thrust::get<2>(ans) = thrust::get<2>(x) / static_cast<float>(y);
return ans;
}
};

}

0 comments on commit 9fd8171

Please sign in to comment.