VDBVolume.cpp

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);
}