Created
October 13, 2022 06:36
-
-
Save mpottinger/7dcdd68c1113f81c8e196f7874ea2e64 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
namespace vdbfusion { | |
VDBVolume::VDBVolume(float voxel_size, float sdf_trunc, bool space_carving /* = false*/) | |
: voxel_size_(voxel_size), sdf_trunc_(sdf_trunc), space_carving_(space_carving) { | |
tsdf_ = openvdb::FloatGrid::create(sdf_trunc_); | |
tsdf_->setName("D(x): signed distance grid"); | |
tsdf_->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size_)); | |
tsdf_->setGridClass(openvdb::GRID_LEVEL_SET); | |
weights_ = openvdb::FloatGrid::create(0.0f); | |
weights_->setName("W(x): weights grid"); | |
weights_->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size_)); | |
weights_->setGridClass(openvdb::GRID_UNKNOWN); | |
colors_ = openvdb::Vec3IGrid::create(openvdb::Vec3I(0.0f, 0.0f, 0.0f)); | |
colors_->setName("C(x): colors grid"); | |
colors_->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size_)); | |
colors_->setGridClass(openvdb::GRID_UNKNOWN); | |
// was_updated_ is a grid that stores whether the voxel has been updated since viewing live pointcloud (for accelerating live pointcloud vizualization) | |
was_updated_ = openvdb::BoolGrid::create(false); | |
// was_viewed is a grid that stores whether the voxel has had a point extracted from it (for accelerating live pointcloud vizualization) | |
was_viewed_ = openvdb::FloatGrid::create(sdf_trunc_); | |
} | |
// GetNormal - gets the normal of a voxel in the TSDF volume | |
Eigen::Vector3d VDBVolume::GetNormal(const openvdb::Coord &voxel) const { | |
const auto &xform = tsdf_->transform(); | |
const auto voxel_center = GetVoxelCenter(voxel, xform); | |
const auto voxel_size = xform.voxelSize(); | |
const auto voxel_size_x = voxel_size.x(); | |
const auto voxel_size_y = voxel_size.y(); | |
const auto voxel_size_z = voxel_size.z(); | |
const auto tsdf_acc = tsdf_->getConstAccessor(); | |
const auto tsdf = tsdf_acc.getValue(voxel); | |
const auto x = voxel.x(); | |
const auto y = voxel.y(); | |
const auto z = voxel.z(); | |
const auto tsdf_xp = tsdf_acc.getValue(openvdb::Coord(x + 1, y, z)); | |
const auto tsdf_xn = tsdf_acc.getValue(openvdb::Coord(x - 1, y, z)); | |
const auto tsdf_yp = tsdf_acc.getValue(openvdb::Coord(x, y + 1, z)); | |
const auto tsdf_yn = tsdf_acc.getValue(openvdb::Coord(x, y - 1, z)); | |
const auto tsdf_zp = tsdf_acc.getValue(openvdb::Coord(x, y, z + 1)); | |
const auto tsdf_zn = tsdf_acc.getValue(openvdb::Coord(x, y, z - 1)); | |
const auto dx = (tsdf_xp - tsdf_xn) / (2 * voxel_size_x); | |
const auto dy = (tsdf_yp - tsdf_yn) / (2 * voxel_size_y); | |
const auto dz = (tsdf_zp - tsdf_zn) / (2 * voxel_size_z); | |
const auto normal = Eigen::Vector3d(dx, dy, dz); | |
return normal.normalized(); | |
} | |
// extract a color point cloud from the TSDF volume (extracts ALL points, a faster live preview but lower quality version below) | |
std::tuple<std::vector<Eigen::Vector3d>,std::vector<Eigen::Vector3d>,std::vector<Eigen::Vector3d>> VDBVolume::ExtractPointCloud(float thresh, float min_weight) const { | |
std::vector<Eigen::Vector3d> points; | |
std::vector<Eigen::Vector3d> colors; | |
std::vector<Eigen::Vector3d> normals; | |
auto weights_acc = weights_->getConstAccessor(); | |
auto colors_acc = colors_->getConstAccessor(); | |
for (auto iter = tsdf_->cbeginValueOn(); iter.test(); ++iter) { | |
const auto &voxel = iter.getCoord(); | |
// get tsdf value and convert tsdf to positive distance | |
const auto &tsdf = iter.getValue(); | |
const auto tsdf_abs = std::abs(tsdf); | |
if (tsdf_abs < thresh) { | |
const auto& weight = weights_acc.getValue(voxel); | |
if (weight > min_weight) { | |
const auto& color = colors_acc.getValue(voxel); | |
const auto point = GetVoxelCenter(voxel, tsdf_->transform()); | |
// get normal from tsdf | |
const auto normal = GetNormal(voxel); | |
points.push_back(Eigen::Vector3d(point.x(), point.y(), point.z())); | |
colors.push_back(Eigen::Vector3d(color.x(), color.y(), color.z())); | |
normals.push_back(normal); | |
} | |
} | |
} | |
return std::make_tuple(points, colors, normals); | |
} | |
std::tuple<std::vector<Eigen::Vector3d>,std::vector<Eigen::Vector3d>> VDBVolume::ExtractPointCloudLivePreview(float thresh, float min_weight) const { | |
// does the same as ExtractPointCloud but only extracts voxels that are marked as was_updated_ == true and was_viewed_ == false | |
std::vector<Eigen::Vector3d> points; | |
std::vector<Eigen::Vector3d> colors; | |
auto tsdf_acc = tsdf_->getConstAccessor(); | |
auto weights_acc = weights_->getConstAccessor(); | |
auto colors_acc = colors_->getConstAccessor(); | |
auto was_viewed_acc = was_viewed_->getUnsafeAccessor(); | |
auto was_updated_acc = was_updated_->getUnsafeAccessor(); | |
// iterate over was_updated_ grid instead of tsdf_ grid | |
for (auto iter = was_updated_->beginValueOn(); iter.test(); ++iter) { | |
const auto &voxel = iter.getCoord(); | |
//iter.setValueOff(); // set that we've already processed this voxel | |
iter.setValue(false); // set that we've already processed this voxel | |
iter.setValueOff(); // set that we've already processed this voxel | |
// get tsdf value and convert tsdf to positive distance | |
const auto &tsdf = tsdf_acc.getValue(voxel); | |
const auto tsdf_abs = std::abs(tsdf); | |
const auto &viewed_tsdf = was_viewed_acc.getValue(voxel); | |
// if viewed value is close to the tsdf value, then we've already processed this voxel | |
const auto diff = std::abs(viewed_tsdf - tsdf); | |
if (diff < sdf_trunc_ / 2) { // if tsdf value is close enough to last seen, don't try to extract a point. | |
continue; | |
} | |
// set was_viewed_ to true so that this voxel is not extracted again | |
was_viewed_acc.setValue(voxel, tsdf); | |
if (tsdf_abs < thresh) { | |
const auto& weight = weights_acc.getValue(voxel); | |
if (weight > min_weight) { | |
const auto& color = colors_acc.getValue(voxel); | |
const auto point = GetVoxelCenter(voxel, tsdf_->transform()); | |
// TODO: add bloom filter as second step to make this even more efficient | |
// (prevent duplicate points more effectively) | |
points.push_back(Eigen::Vector3d(point.x(), point.y(), point.z())); | |
colors.push_back(Eigen::Vector3d(color.x(), color.y(), color.z())); | |
} | |
} | |
} | |
return std::make_tuple(points, colors); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment