Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 14 additions & 2 deletions cpp/patchworkpp/include/patchwork/patchworkpp.h
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@

#include <iostream>
#include <numeric>
#include <stdexcept>
#include <vector>

#include <Eigen/Dense>
Expand Down Expand Up @@ -126,6 +127,17 @@ class PatchWorkpp {
typedef std::vector<Ring> Zone;

PatchWorkpp(patchwork::Params _params) : params_(_params) {
const int total_rings =
std::accumulate(params_.num_rings_each_zone.begin(), params_.num_rings_each_zone.end(), 0);
if (params_.num_rings_of_interest < 0 || params_.num_rings_of_interest > total_rings) {
throw std::invalid_argument(
"num_rings_of_interest must be between 0 and the total ring count");
}
// Adaptive thresholds and histories need one entry per checked ring.
params_.elevation_thr.resize(params_.num_rings_of_interest, 0.0);
params_.flatness_thr.resize(params_.num_rings_of_interest, 0.0);
update_elevation_.resize(params_.num_rings_of_interest);
update_flatness_.resize(params_.num_rings_of_interest);
double min_range_z2_ = (7 * params_.min_range + params_.max_range) / 8.0;
double min_range_z3_ = (3 * params_.min_range + params_.max_range) / 4.0;
double min_range_z4_ = (params_.min_range + params_.max_range) / 2.0;
Expand Down Expand Up @@ -176,8 +188,8 @@ class PatchWorkpp {
time_t timer_;
long time_taken_;

std::vector<double> update_flatness_[4];
std::vector<double> update_elevation_[4];
std::vector<std::vector<double>> update_flatness_;
std::vector<std::vector<double>> update_elevation_;

double d_;

Expand Down
25 changes: 18 additions & 7 deletions python/tests/test_smoke.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import numpy as np
import pypatchworkpp
import pytest

DATA_DIR = os.path.join(os.path.dirname(__file__), "..", "..", "data")

Expand All @@ -15,16 +16,26 @@ def test_module_imports_and_exposes_api():
assert hasattr(pypatchworkpp, "patchworkpp")


def test_estimate_ground_partitions_all_points():
@pytest.mark.parametrize("checked_rings", [0, 4, 6, 14])
def test_estimate_ground_partitions_all_points(checked_rings):
params = pypatchworkpp.Parameters()
params.num_rings_of_interest = checked_rings
pp = pypatchworkpp.patchworkpp(params)

scan = _read_bin(os.path.join(DATA_DIR, "000000.bin"))
pp.estimateGround(scan)
for _ in range(2):
pp.estimateGround(scan)
ground = pp.getGround()
nonground = pp.getNonground()

ground = pp.getGround()
nonground = pp.getNonground()
assert ground.shape[0] > 0
assert nonground.shape[0] > 0
assert ground.shape[0] + nonground.shape[0] <= scan.shape[0]

assert ground.shape[0] > 0
assert nonground.shape[0] > 0
assert ground.shape[0] + nonground.shape[0] <= scan.shape[0]

@pytest.mark.parametrize("checked_rings", [-1, 15])
def test_invalid_checked_ring_count(checked_rings):
params = pypatchworkpp.Parameters()
params.num_rings_of_interest = checked_rings
with pytest.raises(ValueError, match="num_rings_of_interest"):
pypatchworkpp.patchworkpp(params)