parent
b944b19159
commit
5c389cdf4a
|
@ -1,46 +0,0 @@
|
||||||
/*
|
|
||||||
* Copyright 2016 The Cartographer Authors
|
|
||||||
*
|
|
||||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
||||||
* you may not use this file except in compliance with the License.
|
|
||||||
* You may obtain a copy of the License at
|
|
||||||
*
|
|
||||||
* http://www.apache.org/licenses/LICENSE-2.0
|
|
||||||
*
|
|
||||||
* Unless required by applicable law or agreed to in writing, software
|
|
||||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
||||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
||||||
* See the License for the specific language governing permissions and
|
|
||||||
* limitations under the License.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "cartographer/mapping/submaps.h"
|
|
||||||
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
#include "cartographer/common/port.h"
|
|
||||||
#include "cartographer/transform/transform.h"
|
|
||||||
|
|
||||||
namespace cartographer {
|
|
||||||
namespace mapping {
|
|
||||||
|
|
||||||
Submaps::Submaps() {}
|
|
||||||
|
|
||||||
Submaps::~Submaps() {}
|
|
||||||
|
|
||||||
int Submaps::matching_index() const {
|
|
||||||
if (size() > 1) {
|
|
||||||
return size() - 2;
|
|
||||||
}
|
|
||||||
return size() - 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<int> Submaps::insertion_indices() const {
|
|
||||||
if (size() > 1) {
|
|
||||||
return {size() - 2, size() - 1};
|
|
||||||
}
|
|
||||||
return {size() - 1};
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace mapping
|
|
||||||
} // namespace cartographer
|
|
|
@ -80,42 +80,6 @@ class Submap {
|
||||||
int num_range_data_ = 0;
|
int num_range_data_ = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Submaps is a sequence of maps to which scans are matched and into which scans
|
|
||||||
// are inserted.
|
|
||||||
//
|
|
||||||
// Except during initialization when only a single submap exists, there are
|
|
||||||
// always two submaps into which scans are inserted: an old submap that is used
|
|
||||||
// for matching, and a new one, which will be used for matching next, that is
|
|
||||||
// being initialized.
|
|
||||||
//
|
|
||||||
// Once a certain number of scans have been inserted, the new submap is
|
|
||||||
// considered initialized: the old submap is no longer changed, the "new" submap
|
|
||||||
// is now the "old" submap and is used for scan-to-map matching. Moreover,
|
|
||||||
// a "new" submap gets inserted.
|
|
||||||
class Submaps {
|
|
||||||
public:
|
|
||||||
Submaps();
|
|
||||||
virtual ~Submaps();
|
|
||||||
|
|
||||||
Submaps(const Submaps&) = delete;
|
|
||||||
Submaps& operator=(const Submaps&) = delete;
|
|
||||||
|
|
||||||
// Returns the index of the newest initialized Submap which can be
|
|
||||||
// used for scan-to-map matching.
|
|
||||||
int matching_index() const;
|
|
||||||
|
|
||||||
// Returns the indices of the Submap into which point clouds will
|
|
||||||
// be inserted.
|
|
||||||
std::vector<int> insertion_indices() const;
|
|
||||||
|
|
||||||
// Returns the Submap with the given 'index'. The same 'index' will always
|
|
||||||
// return the same pointer, so that Submaps can be identified by it.
|
|
||||||
virtual const Submap* Get(int index) const = 0;
|
|
||||||
|
|
||||||
// Returns the number of Submaps.
|
|
||||||
virtual int size() const = 0;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
|
|
|
@ -180,6 +180,20 @@ const Submap* Submaps::Get(int index) const {
|
||||||
|
|
||||||
int Submaps::size() const { return submaps_.size(); }
|
int Submaps::size() const { return submaps_.size(); }
|
||||||
|
|
||||||
|
int Submaps::matching_index() const {
|
||||||
|
if (size() > 1) {
|
||||||
|
return size() - 2;
|
||||||
|
}
|
||||||
|
return size() - 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<int> Submaps::insertion_indices() const {
|
||||||
|
if (size() > 1) {
|
||||||
|
return {size() - 2, size() - 1};
|
||||||
|
}
|
||||||
|
return {size() - 1};
|
||||||
|
}
|
||||||
|
|
||||||
void Submaps::FinishSubmap(int index) {
|
void Submaps::FinishSubmap(int index) {
|
||||||
// Crop the finished Submap before inserting a new Submap to reduce peak
|
// Crop the finished Submap before inserting a new Submap to reduce peak
|
||||||
// memory usage a bit.
|
// memory usage a bit.
|
||||||
|
|
|
@ -60,16 +60,35 @@ class Submap : public mapping::Submap {
|
||||||
bool finished_ = false;
|
bool finished_ = false;
|
||||||
};
|
};
|
||||||
|
|
||||||
// A container of Submaps.
|
// Submaps is a sequence of maps to which scans are matched and into which scans
|
||||||
class Submaps : public mapping::Submaps {
|
// are inserted.
|
||||||
|
//
|
||||||
|
// Except during initialization when only a single submap exists, there are
|
||||||
|
// always two submaps into which scans are inserted: an old submap that is used
|
||||||
|
// for matching, and a new one, which will be used for matching next, that is
|
||||||
|
// being initialized.
|
||||||
|
//
|
||||||
|
// Once a certain number of scans have been inserted, the new submap is
|
||||||
|
// considered initialized: the old submap is no longer changed, the "new" submap
|
||||||
|
// is now the "old" submap and is used for scan-to-map matching. Moreover,
|
||||||
|
// a "new" submap gets inserted.
|
||||||
|
class Submaps {
|
||||||
public:
|
public:
|
||||||
explicit Submaps(const proto::SubmapsOptions& options);
|
explicit Submaps(const proto::SubmapsOptions& options);
|
||||||
|
|
||||||
Submaps(const Submaps&) = delete;
|
Submaps(const Submaps&) = delete;
|
||||||
Submaps& operator=(const Submaps&) = delete;
|
Submaps& operator=(const Submaps&) = delete;
|
||||||
|
|
||||||
const Submap* Get(int index) const override;
|
const Submap* Get(int index) const;
|
||||||
int size() const override;
|
int size() const;
|
||||||
|
|
||||||
|
// Returns the index of the newest initialized Submap which can be
|
||||||
|
// used for scan-to-map matching.
|
||||||
|
int matching_index() const;
|
||||||
|
|
||||||
|
// Returns the indices of the Submap into which point clouds will
|
||||||
|
// be inserted.
|
||||||
|
std::vector<int> insertion_indices() const;
|
||||||
|
|
||||||
// Inserts 'range_data' into the Submap collection.
|
// Inserts 'range_data' into the Submap collection.
|
||||||
void InsertRangeData(const sensor::RangeData& range_data);
|
void InsertRangeData(const sensor::RangeData& range_data);
|
||||||
|
|
|
@ -379,6 +379,20 @@ const Submap* Submaps::Get(int index) const {
|
||||||
|
|
||||||
int Submaps::size() const { return submaps_.size(); }
|
int Submaps::size() const { return submaps_.size(); }
|
||||||
|
|
||||||
|
int Submaps::matching_index() const {
|
||||||
|
if (size() > 1) {
|
||||||
|
return size() - 2;
|
||||||
|
}
|
||||||
|
return size() - 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<int> Submaps::insertion_indices() const {
|
||||||
|
if (size() > 1) {
|
||||||
|
return {size() - 2, size() - 1};
|
||||||
|
}
|
||||||
|
return {size() - 1};
|
||||||
|
}
|
||||||
|
|
||||||
void Submaps::InsertRangeData(const sensor::RangeData& range_data,
|
void Submaps::InsertRangeData(const sensor::RangeData& range_data,
|
||||||
const Eigen::Quaterniond& gravity_alignment) {
|
const Eigen::Quaterniond& gravity_alignment) {
|
||||||
for (const int index : insertion_indices()) {
|
for (const int index : insertion_indices()) {
|
||||||
|
|
|
@ -73,16 +73,35 @@ class Submap : public mapping::Submap {
|
||||||
bool finished_ = false;
|
bool finished_ = false;
|
||||||
};
|
};
|
||||||
|
|
||||||
// A container of Submaps.
|
// Submaps is a sequence of maps to which scans are matched and into which scans
|
||||||
class Submaps : public mapping::Submaps {
|
// are inserted.
|
||||||
|
//
|
||||||
|
// Except during initialization when only a single submap exists, there are
|
||||||
|
// always two submaps into which scans are inserted: an old submap that is used
|
||||||
|
// for matching, and a new one, which will be used for matching next, that is
|
||||||
|
// being initialized.
|
||||||
|
//
|
||||||
|
// Once a certain number of scans have been inserted, the new submap is
|
||||||
|
// considered initialized: the old submap is no longer changed, the "new" submap
|
||||||
|
// is now the "old" submap and is used for scan-to-map matching. Moreover,
|
||||||
|
// a "new" submap gets inserted.
|
||||||
|
class Submaps {
|
||||||
public:
|
public:
|
||||||
explicit Submaps(const proto::SubmapsOptions& options);
|
explicit Submaps(const proto::SubmapsOptions& options);
|
||||||
|
|
||||||
Submaps(const Submaps&) = delete;
|
Submaps(const Submaps&) = delete;
|
||||||
Submaps& operator=(const Submaps&) = delete;
|
Submaps& operator=(const Submaps&) = delete;
|
||||||
|
|
||||||
const Submap* Get(int index) const override;
|
const Submap* Get(int index) const;
|
||||||
int size() const override;
|
int size() const;
|
||||||
|
|
||||||
|
// Returns the index of the newest initialized Submap which can be
|
||||||
|
// used for scan-to-map matching.
|
||||||
|
int matching_index() const;
|
||||||
|
|
||||||
|
// Returns the indices of the Submap into which point clouds will
|
||||||
|
// be inserted.
|
||||||
|
std::vector<int> insertion_indices() const;
|
||||||
|
|
||||||
// Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
|
// Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
|
||||||
// used for the orientation of new submaps so that the z axis approximately
|
// used for the orientation of new submaps so that the z axis approximately
|
||||||
|
|
Loading…
Reference in New Issue