probability_values_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include "gtest/gtest.h"
20 
21 namespace cartographer {
22 namespace mapping {
23 namespace {
24 
25 TEST(ProbabilityValuesTest, OddsConversions) {
27  1e-6);
29  1e-6);
30  EXPECT_NEAR(ProbabilityFromOdds(Odds(0.5)), 0.5, 1e-6);
31 }
32 
33 TEST(ProbabilityValuesTest, OddsConversionsCorrespondenceCost) {
42  0.5, 1e-6);
43 }
44 
45 TEST(ProbabilityValuesTest,
46  ProbabilityValueToCorrespondenceCostValueConversions) {
47  for (uint16 i = 0; i < 32768; ++i) {
50  i);
53  i);
54  }
55 }
56 
57 TEST(ProbabilityValuesTest,
58  ProbabilityValueToCorrespondenceCostValueConversionsWithUpdateMarker) {
59  for (uint16 i = 1; i < 32768; ++i) {
62  i + kUpdateMarker);
65  i + kUpdateMarker);
66  }
67 }
68 
69 TEST(ProbabilityValuesTest, ConversionLookUpTable) {
70  EXPECT_NEAR(ValueToProbability(0), 1.f - ValueToCorrespondenceCost(0), 1e-6);
71  for (uint16 i = 1; i < 32768; ++i) {
72  EXPECT_NEAR(ValueToProbability(i), ValueToCorrespondenceCost(i), 1e-6)
73  << " i " << i;
74  }
75 }
76 
77 TEST(ProbabilityValuesTest, CellUpdate) {
78  std::vector<uint16> probability_table =
80  std::vector<uint16> correspondence_table =
82  uint16 cell_pg_pre_update = 0;
83  uint16 cell_cg_pre_update = 0;
84  uint16 cell_pg_post_update = probability_table[cell_pg_pre_update];
85  uint16 cell_cg_post_update = correspondence_table[cell_cg_pre_update];
86  float p_post = ValueToProbability(cell_pg_post_update);
87  float c_post = ValueToCorrespondenceCost(cell_cg_post_update);
88  EXPECT_NEAR(p_post, 1.f - c_post, 1e-6);
89  int num_evaluations = 5000;
90  for (int i_probability = 0; i_probability < num_evaluations;
91  ++i_probability) {
92  float p = (static_cast<float>(i_probability) /
93  static_cast<float>(num_evaluations)) *
96  cell_pg_pre_update = ProbabilityToValue(p);
97  cell_cg_pre_update =
99  float p_value =
101  (32766.f / (kMaxProbability - kMinProbability));
104  kMinProbability) *
105  (32766.f / (kMaxProbability - kMinProbability));
106 
107  EXPECT_NEAR(cell_pg_pre_update, 32768 - cell_cg_pre_update, 1)
108  << "p " << p << " p_value " << p_value << " cvalue " << cvalue
109  << " p_valuei " << common::RoundToInt(p_value) << " cvaluei "
110  << common::RoundToInt(cvalue);
111  cell_pg_post_update = probability_table[cell_pg_pre_update];
112  cell_cg_post_update = correspondence_table[cell_cg_pre_update];
113  p_post = ValueToProbability(cell_pg_post_update);
114  c_post = ValueToCorrespondenceCost(cell_cg_post_update);
115  EXPECT_NEAR(p_post, 1.f - c_post, 5e-5)
116  << "p " << p << " " << p_post - 1.f + c_post;
117  }
118 }
119 
120 TEST(ProbabilityValuesTest, MultipleCellUpdate) {
121  std::vector<uint16> probability_table =
123  std::vector<uint16> correspondence_table =
125  uint16 cell_pg_post_update = probability_table[0];
126  uint16 cell_cg_post_update = correspondence_table[0];
127  float p_post = ValueToProbability(cell_pg_post_update);
128  float c_post = ValueToCorrespondenceCost(cell_cg_post_update);
129  EXPECT_NEAR(p_post, 1.f - c_post, 1e-6);
130  int num_evaluations = 5000;
131  for (int i_probability = 0; i_probability < num_evaluations;
132  ++i_probability) {
133  float p = (static_cast<float>(i_probability) /
134  static_cast<float>(num_evaluations)) *
137  cell_pg_post_update = ProbabilityToValue(p) + kUpdateMarker;
138  cell_cg_post_update =
141  for (int i_update = 0; i_update < 20; ++i_update) {
142  cell_pg_post_update =
143  probability_table[cell_pg_post_update - kUpdateMarker];
144  cell_cg_post_update =
145  correspondence_table[cell_cg_post_update - kUpdateMarker];
146  }
147  p_post = ValueToProbability(cell_pg_post_update);
148  c_post = ValueToCorrespondenceCost(cell_cg_post_update);
149  EXPECT_NEAR(p_post, 1.f - c_post, 5e-5)
150  << "p " << p << " p_post " << p_post << " " << p_post - 1.f + c_post;
151  }
152 }
153 
154 TEST(ProbabilityValuesTest, EqualityLookupTableToApplyOdds) {
155  std::vector<uint16> probability_table = ComputeLookupTableToApplyOdds(0.3);
156  std::vector<uint16> correspondence_table =
158  for (int i = 0; i < 32768; ++i) {
159  EXPECT_NEAR(
160  probability_table[i],
162  correspondence_table[ProbabilityValueToCorrespondenceCostValue(i)]),
163  1);
164  EXPECT_NEAR(
166  probability_table[CorrespondenceCostValueToProbabilityValue(i)]),
167  correspondence_table[i], 1);
168  }
169 }
170 
171 } // namespace
172 } // namespace mapping
173 } // namespace cartographer
uint16 ProbabilityValueToCorrespondenceCostValue(uint16 probability_value)
std::vector< uint16 > ComputeLookupTableToApplyOdds(const float odds)
std::vector< uint16 > ComputeLookupTableToApplyCorrespondenceCostOdds(float odds)
float ProbabilityFromOdds(const float odds)
float ValueToCorrespondenceCost(const uint16 value)
int RoundToInt(const float x)
Definition: port.h:41
constexpr float kMinProbability
constexpr float kMaxCorrespondenceCost
float ValueToProbability(const uint16 value)
constexpr float kMaxProbability
float CorrespondenceCostToProbability(const float correspondence_cost)
uint16 CorrespondenceCostValueToProbabilityValue(uint16 correspondence_cost_value)
uint16_t uint16
Definition: port.h:35
float Odds(float probability)
float ProbabilityToCorrespondenceCost(const float probability)
uint16 ProbabilityToValue(const float probability)
T Clamp(const T value, const T min, const T max)
Definition: math.h:32
uint16 CorrespondenceCostToValue(const float correspondence_cost)
constexpr uint16 kUpdateMarker
constexpr float kMinCorrespondenceCost
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory)


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58