Skip to content

Commit

Permalink
Dubins Classification Speed ups
Browse files Browse the repository at this point in the history 
- Changes to optimize a_22/a_33
- Minor changes in other Dubins classifications and removed some default edge cases where it would resort to all path types.
  • Loading branch information
@bradygm
bradygm committed Sep 19, 2023
1 parent 3197963 commit e4c71c4
Show file tree
Hide file tree
Showing 3 changed files with 166 additions and 44 deletions.
67 changes: 23 additions & 44 deletions src/DubinsStateSpace.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -278,10 +278,10 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
DubinsPath dubinsrsl = dubinsRSL(d, alpha, beta);
double S13 = dubinsrsr.length_[0] - M_PI;
double S12 = dubinsrsr.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[2] - M_PI);

if (S13 <= 0)
{
double S12 = dubinsrsr.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[2] - M_PI);
if (S12 > 0)
{
return dubinsrsl;
Expand Down Expand Up @@ -327,10 +327,10 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
DubinsPath dubinsrsl = dubinsRSL(d, alpha, beta);
double S31 = dubinslsl.length_[2] - M_PI;
double S21 = dubinslsl.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[0] - M_PI);

if (S31 <= 0)
{
double S21 = dubinslsl.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[0] - M_PI);
if (S21 > 0)
{
return dubinsrsl;
Expand Down Expand Up @@ -362,13 +362,9 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
{
return dubinsrsr;
}
else if (S24 > 0)
{
return dubinsRSL(d, alpha, beta);
}
else
{
return Dubins::dubins(d, alpha, beta);
return dubinsRSL(d, alpha, beta);
}
}
else if (init_quadrant == 3 && final_quadrant == 1)
Expand All @@ -380,24 +376,20 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
{
return dubinslsl;
}
else if (S31 > 0)
{
return dubinsLSR(d, alpha, beta);
}
else
{
return Dubins::dubins(d, alpha, beta);
return dubinsLSR(d, alpha, beta);
}
}
else if (init_quadrant == 3 && final_quadrant == 4)
{
DubinsPath dubinslsr = dubinsLSR(d, alpha, beta);
DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
double S24 = dubinsrsr.length_[2] - M_PI;
double S34 = dubinsrsr.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[0] - M_PI);

if (S24 <= 0)
{
double S34 = dubinsrsr.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[0] - M_PI);
if (S34 > 0)
{
return dubinslsr;
Expand Down Expand Up @@ -429,24 +421,20 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
{
return dubinslsl;
}
else if (S42 > 0)
{
return dubinsRSL(d, alpha, beta);
}
else
{
return Dubins::dubins(d, alpha, beta);
return dubinsRSL(d, alpha, beta);
}
}
else if (init_quadrant == 4 && final_quadrant == 3)
{
DubinsPath dubinslsr = dubinsLSR(d, alpha, beta);
DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
double S42 = dubinslsl.length_[0] - M_PI;
double S43 = dubinslsl.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[2] - M_PI);

if (S42 <= 0)
{
double S43 = dubinslsl.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[2] - M_PI);
if (S43 > 0)
{
return dubinslsr;
Expand Down Expand Up @@ -492,18 +480,14 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
}
else if (init_quadrant == 2 && final_quadrant == 2)
{
DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
DubinsPath dubinsrsl = dubinsRSL(d, alpha, beta);
DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);

double plsl = dubinslsl.length_[1];
double prsl = dubinsrsl.length_[1];
double trsl = dubinsrsl.length_[0];
double prsr = dubinsrsr.length_[1];
double qrsl = dubinsrsl.length_[2];


if (alpha > beta)
{
DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
double trsl = dubinsrsl.length_[0];
double plsl = dubinslsl.length_[1];
double S122 = plsl - prsl - 2*(trsl - M_PI);
if (S122 < 0)
{
Expand All @@ -518,8 +502,11 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
return Dubins::dubins(d, alpha, beta);
}
}
else if (alpha < beta)
else
{
DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
double prsr = dubinsrsr.length_[1];
double qrsl = dubinsrsl.length_[2];
double S222 = prsr - prsl - 2*(qrsl - M_PI);
if (S222 < 0)
{
Expand All @@ -534,24 +521,17 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
return Dubins::dubins(d, alpha, beta);
}
}
else
{
return Dubins::dubins(d, alpha, beta);
}
}
else if (init_quadrant == 3 && final_quadrant == 3)
{
DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
DubinsPath dubinslsr = dubinsLSR(d, alpha, beta);
DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);

double prsr = dubinsrsr.length_[1];
double plsr = dubinslsr.length_[1];
double tlsr = dubinslsr.length_[0];
double plsl = dubinslsl.length_[1];
double qlsr = dubinslsr.length_[2];

if (alpha < beta)
{
DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
double prsr = dubinsrsr.length_[1];
double tlsr = dubinslsr.length_[0];
double S133 = prsr - plsr - 2*(tlsr - M_PI);
if (S133 < 0)
{
Expand All @@ -566,8 +546,11 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
return Dubins::dubins(d, alpha, beta);
}
}
else if (alpha > beta)
else
{
DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
double plsl = dubinslsl.length_[1];
double qlsr = dubinslsr.length_[2];
double S233 = plsl - plsr - 2*(qlsr - M_PI);
if (S233 < 0)
{
Expand All @@ -582,10 +565,6 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
return Dubins::dubins(d, alpha, beta);
}
}
else
{
return Dubins::dubins(d, alpha, beta);
}
}
else if (init_quadrant == 4 && final_quadrant == 1)
{
Expand Down
59 changes: 59 additions & 0 deletions test/all_benchmarks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -481,5 +481,64 @@ static void BM_Dubins_Random_Classification(benchmark::State& state) {
}
BENCHMARK(BM_Dubins_Random_Classification);

static void BM_Dubins_a12(benchmark::State& state) {
double max_kappa = 0.01;

Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);

for (auto _ : state){
benchmark::DoNotOptimize(dubins_path_object.dubins({0, 0, 0.5}, {800, 0, 2.4}));
benchmark::ClobberMemory();
}

}
BENCHMARK(BM_Dubins_a12);

static void BM_Dubins_a12_random(benchmark::State& state) {
std::random_device rd;
std::mt19937 gen = std::mt19937(rd());
std::uniform_real_distribution<> disPhi1(0.0, M_PI_2);
std::uniform_real_distribution<> disPhi2(M_PI_2, M_PI);
double max_kappa = 0.01;

Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);

for (auto _ : state){
benchmark::DoNotOptimize(dubins_path_object.dubins_matrix({0, 0, disPhi1(gen)}, {800, 0, disPhi2(gen)}));
benchmark::ClobberMemory();
}

}
BENCHMARK(BM_Dubins_a12_random);

static void BM_Dubins_a22(benchmark::State& state) {
double max_kappa = 0.01;

Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);

for (auto _ : state){
benchmark::DoNotOptimize(dubins_path_object.dubins({0, 0, 2.8}, {800, 0, 2.4}));
benchmark::ClobberMemory();
}

}
BENCHMARK(BM_Dubins_a22);

static void BM_Dubins_a22_random(benchmark::State& state) {
std::random_device rd;
std::mt19937 gen = std::mt19937(rd());
std::uniform_real_distribution<> disPhi2(M_PI_2, M_PI);
double max_kappa = 0.01;

Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);

for (auto _ : state){
benchmark::DoNotOptimize(dubins_path_object.dubins_matrix({0, 0, disPhi2(gen)}, {800, 0, disPhi2(gen)}));
benchmark::ClobberMemory();
}

}
BENCHMARK(BM_Dubins_a22_random);


BENCHMARK_MAIN();
84 changes: 84 additions & 0 deletions test/unit_test_trochoid_classification.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -487,6 +487,90 @@ TEST_F(DubinsTestFixture, failure_case_dubins_four_r1_2)
EXPECT_TRUE(abs(dubins_matrix_path_length - dubins_path_length) < 1e-5);
}

TEST_F(DubinsTestFixture, dubins_a12)
{
/*
Dubins Matrix Path Length: 1.79769e+308
Dubins Path Length: 6.60756
Max_Kappa: 0.00982667
Start State: -347.852, 202.189, 3.46437
Goal State: -505.575, 148.801, 0.333584
Dubins Matrix Path Type: 0, 1, 0
Dubins Path Type: 2, 0, 2
*/
max_kappa = 0.01;
// Run dubins on this (should choose dubinsLSL and segfault)

Dubins::DubinsStateSpace::DubinsState start_state = {0, 0, 0.5};
Dubins::DubinsStateSpace::DubinsState goal_state = {800, 0, 2.4};

Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);

dubins_path_matrix = dubins_path_object.dubins_matrix(start_state, goal_state);
dubins_path = dubins_path_object.dubins(start_state, goal_state);

double dubins_matrix_path_length = dubins_path_matrix.length();
double dubins_path_length = dubins_path.length();

EXPECT_TRUE(abs(dubins_matrix_path_length - dubins_path_length) < 1e-5);
}

TEST_F(DubinsTestFixture, dubins_a22)
{
/*
Dubins Matrix Path Length: 1.79769e+308
Dubins Path Length: 6.60756
Max_Kappa: 0.00982667
Start State: -347.852, 202.189, 3.46437
Goal State: -505.575, 148.801, 0.333584
Dubins Matrix Path Type: 0, 1, 0
Dubins Path Type: 2, 0, 2
*/
max_kappa = 0.01;
// Run dubins on this (should choose dubinsLSL and segfault)

Dubins::DubinsStateSpace::DubinsState start_state = {0, 0, 2.8};
Dubins::DubinsStateSpace::DubinsState goal_state = {800, 0, 2.4};

Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);

dubins_path_matrix = dubins_path_object.dubins_matrix(start_state, goal_state);
dubins_path = dubins_path_object.dubins(start_state, goal_state);

double dubins_matrix_path_length = dubins_path_matrix.length();
double dubins_path_length = dubins_path.length();

EXPECT_TRUE(abs(dubins_matrix_path_length - dubins_path_length) < 1e-5);
}

TEST_F(DubinsTestFixture, dubins_a22_equal_alpha_beta)
{
/*
Dubins Matrix Path Length: 1.79769e+308
Dubins Path Length: 6.60756
Max_Kappa: 0.00982667
Start State: -347.852, 202.189, 3.46437
Goal State: -505.575, 148.801, 0.333584
Dubins Matrix Path Type: 0, 1, 0
Dubins Path Type: 2, 0, 2
*/
max_kappa = 0.01;
// Run dubins on this (should choose dubinsLSL and segfault)

Dubins::DubinsStateSpace::DubinsState start_state = {0, 0, 2.4};
Dubins::DubinsStateSpace::DubinsState goal_state = {800, 0, 2.4};

Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);

dubins_path_matrix = dubins_path_object.dubins_matrix(start_state, goal_state);
dubins_path = dubins_path_object.dubins(start_state, goal_state);

double dubins_matrix_path_length = dubins_path_matrix.length();
double dubins_path_length = dubins_path.length();

EXPECT_TRUE(abs(dubins_matrix_path_length - dubins_path_length) < 1e-5);
}

TEST_F(DubinsTestFixture, fail_case_8_tests)
{
max_kappa = 0.066222699999999995;
Expand Down

0 comments on commit e4c71c4

Please sign in to comment.