/
/
/
1#include "fast_slam/MeasurementSet.hpp"
2
3namespace fastslam{
4
5MeasurementSet::MeasurementSet(){
6 first_measurement_node_ = nullptr;
7 n_measurements_ = 0;
8}
9
10// MeasurementSet::MeasurementSet(Measurement *meas){
11// first_measurement_node_ = new MeasurementSetNode;
12// first_measurement_node_->meas = meas;
13// first_measurement_node_->next_node = NULL;
14// nMeas = 1;
15// first_measurement_node_->measIdentifier = nMeas;
16// //cout << "n1: " << nMeas << endl;
17// }
18
19MeasurementSet::~MeasurementSet(){}
20
21void MeasurementSet::addMeasurement(std::shared_ptr<Measurement> measurement){
22 if(first_measurement_node_ == nullptr){
23 std::unique_ptr<MeasurementSetNode> new_node = std::make_unique<MeasurementSetNode>();
24 new_node->measurement = measurement;
25 new_node->next_node = nullptr;
26 new_node->measurement_identifier = n_measurements_ + 1;
27 first_measurement_node_ = std::move(new_node);
28 n_measurements_++;
29
30 }
31 else
32 addMeasurement(first_measurement_node_, measurement);
33}
34
35void MeasurementSet::addMeasurement(std::unique_ptr<MeasurementSetNode> &next_ptr, std::shared_ptr<Measurement> &measurement){
36 if (next_ptr->next_node == nullptr){
37 std::unique_ptr<MeasurementSetNode> new_node = std::make_unique<MeasurementSetNode>();
38 new_node->measurement = measurement;
39 new_node->next_node = nullptr;
40 new_node->measurement_identifier = n_measurements_ + 1;
41 next_ptr->next_node = std::move(new_node);
42 n_measurements_++;
43 }
44 else{
45 addMeasurement(next_ptr->next_node, measurement);
46 }
47}
48
49// int MeasurementSet::countNumberOfMeasurements(){
50
51// if (first_measurement_node_==NULL){
52// nMeas = 0;
53// return 0;
54// }
55// else{
56// int i = 1;
57// MeasurementSetNode* tmp_pointer = first_measurement_node_;
58// while(tmp_pointer->next_node != NULL){
59// tmp_pointer = tmp_pointer->next_node;
60// i++;
61// }
62// nMeas = i;
63// return i;
64// }
65
66// }
67
68
69std::shared_ptr<Measurement> MeasurementSet::getMeasurement(uint32_t identifier) const{
70 return getMeasurement(first_measurement_node_, identifier);
71}
72
73std::shared_ptr<Measurement> MeasurementSet::getMeasurement(const std::unique_ptr<MeasurementSetNode> &next_ptr, uint32_t identifier) const{
74 if(next_ptr->measurement_identifier != identifier)
75 return getMeasurement(next_ptr->next_node, identifier);
76 else
77 return next_ptr->measurement;
78}
79
80
81} //namespace fastslam
82