Make voxel computation consistent across all source code

[saurabh1002]

1. Make all the source code doing voxel computation use a consistent style of converting and type casting to Eigen::Vector3i.
2. Use explicitly the std::floor function from numerics standard library to have control over the rounding off done while converting Eigen::Vector3d to Eigen::Vector3i. As mentioned also in Unintendet behaviour in kiss_icp/cpp/kiss_icp/core/VoxelHashMap.cpp AddPoints ? #191, it avoids the issue of having large voxels around the origin of each axes.

[benemer]

Looks good, thanks! Do you have an estimate of how much it impacts the results on the common datasets?

[saurabh1002]

I am running some experiments now to see the impact on performance. I will report them soon.

[saurabh1002]

All MulRAN Sequences

[benemer]

It looks like the results get slightly worse. However, the old voxel index computation was actually wrong/unsafe, especially if different voxelizations are used throughout the code.

[nachovizzo]

This 20hz degree in performance is a no-go for me; what do you think ??

[benemer]

I'm unsure if the computation time is comparable, but I don't see why this should be much slower.

@saurabh1002, can you verify again using a single thread?

[saurabh1002]

Apollo Dataset

Single Thread result for Riverside03 incoming soon.

[saurabh1002]

This is single threaded with advanced.yaml config on MulRAN Riverside03 sequence @benemer @nachovizzo

[saurabh1002]

The slight drop in frame rate is expected since by fixing the voxel computation, there are more voxels that need to be processed both in the input frame and the map. The earlier voxelization was leading to a larger voxel (double the voxel_size around the axis origin lines), thereby having fewer voxels to process.

[saurabh1002]

A 2D figure for illustration of how Eigen::cast voxelizes the 3D space, without any explicit floor function before type casting.

[l00p3]

Looks goot to me.

[benemer]

Okay, that explanation makes sense and with the single-thread implementation, the effect does not seem to be big. Also, if you want to have a faster runtime from having larger voxels you should increase their size instead.

A general idea that we could follow up would be to try a polar grid for downsampling, this will keep a higher density for close points and a smaller density for far-away points. But that's outside of this PR.

Good job @saurabh1002 :)

[tizianoGuadagnino]

Sorry for the dismissal; I used the power of Eigen::Array to make this change a single line. It looks good to me, anyway.

[nachovizzo]

Before I even go through the implementation and analyze results, etc.

I tested the situation mentioned in the referenced ticket and randomly picked one point, but I don't see any difference.

#include <Eigen/Core>
#include <iostream>

const double voxel_size = 1.0;

inline Eigen::Vector3i PointToVoxel(const Eigen::Vector3d &point) {
    return (point / voxel_size).array().floor().cast<int>();
}

inline Eigen::Vector3i PointToVoxel2(const Eigen::Vector3d &point) {
    return Eigen::Vector3i((point / voxel_size).cast<int>());
}

int main() {
    const Eigen::Vector3d point{0.1, 0.3, 1.9};
    std::cout << "Point     = " << point.transpose() << std::endl;
    std::cout << "Old Voxel = " << PointToVoxel2(point).transpose() << std::endl;
    std::cout << "New Voxel = " << PointToVoxel(point).transpose() << std::endl;
    return 0;
}

Is there any easy isolation test for this?

[saurabh1002]

#include <Eigen/Core>
#include <iostream>

const double voxel_size = 1.0;

inline Eigen::Vector3i PointToVoxel(const Eigen::Vector3d &point)
{
	return (point / voxel_size).array().floor().cast<int>();
}

inline Eigen::Vector3i PointToVoxel2(const Eigen::Vector3d &point)
{
	return Eigen::Vector3i((point / voxel_size).cast<int>());
}

inline Eigen::Vector3i PointToVoxel3(const Eigen::Vector3d &point)
{
	return Eigen::Vector3i(static_cast<int>(point.x() / voxel_size),
						   static_cast<int>(point.y() / voxel_size),
						   static_cast<int>(point.z() / voxel_size));
}

int main()
{
	const Eigen::Vector3d point_1{0, 0, 0.99};
	std::cout << "Point     = " << point_1.transpose() << std::endl;
	std::cout << "Eigen - Floor Array  = " << PointToVoxel(point_1).transpose() << std::endl;
	std::cout << "Eigen - cast int = " << PointToVoxel2(point_1).transpose() << std::endl;
	std::cout << "std - static cast = " << PointToVoxel3(point_1).transpose() << std::endl;

	std::cout << std::endl << std::endl;

	const Eigen::Vector3d point_2{0, 0, -0.99};
	std::cout << "Point     = " << point_2.transpose() << std::endl;
	std::cout << "Eigen - Floor Array  = " << PointToVoxel(point_2).transpose() << std::endl;
	std::cout << "Eigen - cast int = " << PointToVoxel2(point_2).transpose() << std::endl;
	std::cout << "std - static cast = " << PointToVoxel3(point_2).transpose() << std::endl;
	return 0;
}

For a voxel size of 1.0, both [0, 0, 0.99] and [0, 0, -0.99] belong to the voxel [0, 0, 0] in the case of static_cast<int> and Eigen::Vector3d::cast, which is covering a volume which is four tiimes voxel size in 3D.

[saurabh1002]

This behaviour exists along each point coordinate which is in the range of [-voxel_size, voxel_size]

[saurabh1002]

static_cast from standard library rounds off towards zero for an integer cast, which means floor for positive values, and ceil for negative values.

[nachovizzo]

@saurabh1002 Thanks, fantastic explanation!!

I'm only concerned about performance, as this operation is roughly 8 times slower than we have now. Is there any significant advantage of this? Besides correctness?

Edit about performance:

I just quickly benchmarked this voxelization function, and it's on par with what we have now. Maybe the magic Eigen template is not working great, and I don't want to look into 🦖

inline Eigen::Vector3i PointToVoxel4(const Eigen::Vector3d& point) {
    return {static_cast<int>(std::floor(point.x() / voxel_size)),
            static_cast<int>(std::floor(point.y() / voxel_size)),
            static_cast<int>(std::floor(point.z() / voxel_size))};

}

BTW, this finally shed some light on these artifacts. We always knew it was there, but we always blamed the hashing library :)

main

this

@tizianoGuadagnino what's your view on this?

[saurabh1002]

@nachovizzo what is that tool for benchmarking that u used?

[nachovizzo]

@saurabh1002 I pushed my proposal to avoid creating another PR. Feel free to revert

google benchmark

#include <benchmark/benchmark.h>

#include <Eigen/Core>

const double voxel_size = 1.0;

inline Eigen::Vector3i PointToVoxel(const Eigen::Vector3d& point) {
    return (point / voxel_size).array().floor().cast<int>();
}

inline Eigen::Vector3i PointToVoxel4(const Eigen::Vector3d& point) {
    return {static_cast<int>(std::floor(point.x() / voxel_size)),
            static_cast<int>(std::floor(point.y() / voxel_size)),
            static_cast<int>(std::floor(point.z() / voxel_size))};
}

inline Eigen::Vector3i PointToVoxel2(const Eigen::Vector3d& point) {
    return Eigen::Vector3i((point / voxel_size).cast<int>());
}

inline Eigen::Vector3i PointToVoxel3(const Eigen::Vector3d& point) {
    return Eigen::Vector3i(static_cast<int>(point.x() / voxel_size),
                           static_cast<int>(point.y() / voxel_size),
                           static_cast<int>(point.z() / voxel_size));
}

static void BM_PointToVoxel(benchmark::State& state) {
    Eigen::Vector3d point(1.0, 2.0, 3.0);
    for (auto _ : state) {
        auto voxel = PointToVoxel(point);
        benchmark::DoNotOptimize(voxel);
    }
}

static void BM_PointToVoxel2(benchmark::State& state) {
    Eigen::Vector3d point(1.0, 2.0, 3.0);
    for (auto _ : state) {
        auto voxel = PointToVoxel2(point);
        benchmark::DoNotOptimize(voxel);
    }
}

static void BM_PointToVoxel3(benchmark::State& state) {
    Eigen::Vector3d point(1.0, 2.0, 3.0);
    for (auto _ : state) {
        auto voxel = PointToVoxel3(point);
        benchmark::DoNotOptimize(voxel);
    }
}

static void BM_PointToVoxel4(benchmark::State& state) {
    Eigen::Vector3d point(1.0, 2.0, 3.0);
    for (auto _ : state) {
        auto voxel = PointToVoxel3(point);
        benchmark::DoNotOptimize(voxel);
    }
}

BENCHMARK(BM_PointToVoxel);
BENCHMARK(BM_PointToVoxel2);
BENCHMARK(BM_PointToVoxel3);
BENCHMARK(BM_PointToVoxel4);

BENCHMARK_MAIN();

[tizianoGuadagnino]

@saurabh1002 Thanks, fantastic explanation!!

I'm only concerned about performance, as this operation is roughly 8 times slower than we have now. Is there any significant advantage of this? Besides correctness?

Edit about performance:

I just quickly benchmarked this voxelization function, and it's on par with what we have now. Maybe the magic Eigen template is not working great, and I don't want to look into 🦖

inline Eigen::Vector3i PointToVoxel4(const Eigen::Vector3d& point) {
    return {static_cast<int>(std::floor(point.x() / voxel_size)),
            static_cast<int>(std::floor(point.y() / voxel_size)),
            static_cast<int>(std::floor(point.z() / voxel_size))};

}

BTW, this finally shed some light on these artifacts. We always knew it was there, but we always blamed the hashing library :)

main

this

@tizianoGuadagnino what's your view on this?

@nachovizzo I am a bit lost on these benchmark results. Give me the data association between PointToVoxel<1 2 3 4> and the corresponding functions. I am not keen on leaving performances on the table, but the map looks much sexier. I will definitely keep the result of this PR somehow.

[nachovizzo]

@tizianoGuadagnino is on the name of the functions but here we go