Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 6ecf1e80 编写于 作者: L Liangliang Zhang 提交者： Jiangtao Hu
浏览文件
操作

Perception: added base types and conditional clustering.

上级 eafdc8f3
隐藏空白更改
内联 并排
Showing with 1621 addition and 1 deletion
modules/perception/base/BUILD 0 → 100644
浏览文件 @ 6ecf1e80
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "point_cloud",
hdrs = [
"point.h",
"point_cloud.h",
"point_cloud_types.h",
],
deps = [
"@eigen",
],
)
cc_test(
name = "point_cloud_test",
size = "small",
srcs = [
"point_cloud_test.cc",
],
deps = [
":point_cloud",
"@gtest//:main",
],
)
cpplint()
modules/perception/base/point.h 0 → 100644
浏览文件 @ 6ecf1e80
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* 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.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_BASE_POINT_H_
#define MODULES_PERCEPTION_BASE_POINT_H_
#include <cfloat>
#include <cstdint>
#include <limits>
#include <memory>
#include <vector>
namespace apollo {
namespace perception {
namespace base {
template <typename T>
struct Point {
T x = 0;
T y = 0;
T z = 0;
T intensity = 0;
typedef T Type;
};
template <typename T>
struct PointXYZIT : public Point<T> {
double timestamp = 0.0;
};
template <typename T>
struct PointXYZITH : public PointXYZIT<T> {
float height = std::numeric_limits<float>::max();
};
template <typename T>
struct PointXYZITHB : public PointXYZITH<T> {
int32_t beam_id = -1;
};
template <typename T>
struct PointXYZITHBL : public PointXYZITHB<T> {
uint8_t label = 0;
};
typedef Point<float> PointXYZIF;
typedef Point<double> PointXYZID;
typedef PointXYZIT<float> PointXYZITF;
typedef PointXYZIT<double> PointXYZITD;
typedef PointXYZITH<float> PointXYZITHF;
typedef PointXYZITH<double> PointXYZITHD;
typedef PointXYZITHB<float> PointXYZITHBF;
typedef PointXYZITHB<double> PointXYZITHBD;
typedef PointXYZITHBL<float> PointXYZITHBLF;
typedef PointXYZITHBL<double> PointXYZITHBLD;
const std::size_t kDefaultReservePointNum = 50000;
struct PointIndices {
PointIndices() { indices.reserve(kDefaultReservePointNum); }
std::vector<int> indices;
typedef std::shared_ptr<PointIndices> Ptr;
typedef std::shared_ptr<const PointIndices> ConstPtr;
};
template <typename T>
struct Point2D {
T x = 0;
T y = 0;
};
typedef Point2D<int> Point2DI;
typedef Point2D<float> Point2DF;
typedef Point2D<double> Point2DD;
template <typename T>
struct Point3D {
T x = 0;
T y = 0;
T z = 0;
};
typedef Point3D<int> Point3DI;
typedef Point3D<float> Point3DF;
typedef Point3D<double> Point3DD;
} // namespace base
} // namespace perception
} // namespace apollo
#endif // MODULES_PERCEPTION_BASE_POINT_H_
modules/perception/base/point_cloud.h 0 → 100644
浏览文件 @ 6ecf1e80
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* 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.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_BASE_POINT_CLOUD_H_
#define MODULES_PERCEPTION_BASE_POINT_CLOUD_H_
#include <limits>
#include <memory>
#include <utility>
#include <vector>
#include "Eigen/Dense"
#include "modules/perception/base/point.h"
namespace apollo {
namespace perception {
namespace base {
template <class PointT>
class PointCloud {
public:
// @brief default constructor
PointCloud() = default;
// @brief copy constructor
explicit PointCloud(const PointCloud<PointT>& pc)
: width_(pc.width_), height_(pc.height_) {
points_ = pc.points_;
}
// @brief construct from input point cloud and specified indices
PointCloud(const PointCloud<PointT>& pc, const PointIndices& indices) {
CopyPointCloud(pc, indices);
}
PointCloud(const PointCloud<PointT>& pc, const std::vector<int>& indices) {
CopyPointCloud(pc, indices);
}
// @brief construct given width and height for organized point cloud
PointCloud(size_t width, size_t height, PointT point = PointT())
: width_(width), height_(height) {
points_.assign(width_ * height_, point);
}
// @brief destructor
virtual ~PointCloud() = default;
// @brief add points of input cloud, return the self cloud
inline PointCloud& operator+=(const PointCloud<PointT>& rhs) {
points_.insert(points_.end(), rhs.points_.begin(), rhs.points_.end());
width_ = width_ * height_ + rhs.width_ * rhs.height_;
height_ = 1;
return *this;
}
// @brief accessor of point via 2d indices for organized cloud,
// @return nullptr for non-organized cloud
inline const PointT* at(size_t col, size_t row) const {
return IsOrganized() ? &(points_[row * width_ + col]) : nullptr;
}
inline PointT* at(size_t col, size_t row) {
return IsOrganized() ? &(points_[row * width_ + col]) : nullptr;
}
inline const PointT* operator()(size_t col, size_t row) const {
return IsOrganized() ? &(points_[row * width_ + col]) : nullptr;
}
inline PointT* operator()(size_t col, size_t row) {
return IsOrganized() ? &(points_[row * width_ + col]) : nullptr;
}
// @brief whether the cloud is organized
inline bool IsOrganized() const { return height_ > 1; }
// @brief accessor of point cloud height
inline size_t height() const { return height_; }
// @brief accessor of point cloud width
inline size_t width() const { return width_; }
// @brief accessor of point size, wrapper of vector
inline size_t size() const { return points_.size(); }
// @brief reserve function wrapper of vector
inline virtual void reserve(size_t size) { points_.reserve(size); }
// @brief empty function wrapper of vector
inline bool empty() { return points_.empty(); }
// @brief resize function wrapper of vector
inline virtual void resize(size_t size) {
points_.resize(size);
if (size != width_ * height_) {
width_ = size;
height_ = 1;
}
}
// @brief accessor of point via 1d index
inline const PointT& operator[](size_t n) const { return points_[n]; }
inline PointT& operator[](size_t n) { return points_[n]; }
inline const PointT& at(size_t n) const { return points_[n]; }
inline PointT& at(size_t n) { return points_[n]; }
// @brief front accessor wrapper of vector
inline const PointT& front() const { return points_.front(); }
inline PointT& front() { return points_.front(); }
// @brief back accessor wrapper of vector
inline const PointT& back() const { return points_.back(); }
inline PointT& back() { return points_.back(); }
// @brief push_back function wrapper of vector
inline virtual void push_back(const PointT& point) {
points_.push_back(point);
width_ = points_.size();
height_ = 1;
}
// @brief clear function wrapper of vector
inline virtual void clear() {
points_.clear();
width_ = height_ = 0;
}
// @brief swap point given source and target id
inline virtual bool SwapPoint(size_t source_id, size_t target_id) {
if (source_id < points_.size() && target_id < points_.size()) {
std::swap(points_[source_id], points_[target_id]);
width_ = points_.size();
height_ = 1;
return true;
}
return false;
}
// @brief copy point from another point cloud
inline bool CopyPoint(size_t id, size_t rhs_id,
const PointCloud<PointT>& rhs) {
if (id < points_.size() && rhs_id < rhs.points_.size()) {
points_[id] = rhs.points_[rhs_id];
return true;
}
return false;
}
// @brief copy point cloud
inline void CopyPointCloud(const PointCloud<PointT>& rhs,
const PointIndices& indices) {
CopyPointCloud(rhs, indices.indices);
}
template <typename IndexType>
inline void CopyPointCloud(const PointCloud<PointT>& rhs,
const std::vector<IndexType>& indices) {
width_ = indices.size();
height_ = 1;
points_.resize(indices.size());
for (size_t i = 0; i < indices.size(); ++i) {
points_[i] = rhs.points_[indices[i]];
}
}
// @brief swap point cloud
inline void SwapPointCloud(PointCloud<PointT>* rhs) {
points_.swap(rhs->points_);
std::swap(width_, rhs->width_);
std::swap(height_, rhs->height_);
std::swap(sensor_to_world_pose_, rhs->sensor_to_world_pose_);
std::swap(timestamp_, rhs->timestamp_);
}
typename std::vector<PointT>* mutable_points() { return &points_; }
const typename std::vector<PointT>& points() const { return points_; }
// @brief cloud timestamp setter
void set_timestamp(const double timestamp) { timestamp_ = timestamp; }
// @brief cloud timestamp getter
double get_timestamp() { return timestamp_; }
// @brief sensor to world pose setter
void set_sensor_to_world_pose(const Eigen::Affine3d& sensor_to_world_pose) {
sensor_to_world_pose_ = sensor_to_world_pose;
}
// @brief sensor to world pose getter
const Eigen::Affine3d& sensor_to_world_pose() {
return sensor_to_world_pose_;
}
// @brief rotate the point cloud and set rotation part of pose to identity
void RotatePointCloud(bool check_nan = false) {
Eigen::Vector3d point3d;
Eigen::Matrix3d rotation = sensor_to_world_pose_.linear();
for (auto& point : points_) {
if (!check_nan || (!std::isnan(point.x) && !std::isnan(point.y) &&
!std::isnan(point.z))) {
point3d << point.x, point.y, point.z;
point3d = rotation * point3d;
point.x = static_cast<typename PointT::Type>(point3d(0));
point.y = static_cast<typename PointT::Type>(point3d(1));
point.z = static_cast<typename PointT::Type>(point3d(2));
}
}
sensor_to_world_pose_.linear().setIdentity();
}
// @brief transform the point cloud, set the pose to identity
void TransformPointCloud(bool check_nan = false) {
Eigen::Vector3d point3d;
for (auto& point : points_) {
if (!check_nan || (!std::isnan(point.x) && !std::isnan(point.y) &&
!std::isnan(point.z))) {
point3d << point.x, point.y, point.z;
point3d = sensor_to_world_pose_ * point3d;
point.x = static_cast<typename PointT::Type>(point3d(0));
point.y = static_cast<typename PointT::Type>(point3d(1));
point.z = static_cast<typename PointT::Type>(point3d(2));
}
}
sensor_to_world_pose_.setIdentity();
}
// @brief check data member consistency
virtual bool CheckConsistency() const { return true; }
protected:
std::vector<PointT> points_;
size_t width_ = 0;
size_t height_ = 0;
Eigen::Affine3d sensor_to_world_pose_ = Eigen::Affine3d::Identity();
double timestamp_ = 0.0;
};
// @brief Point cloud class split points and attributes storage
// for fast traversing
template <class PointT>
class AttributePointCloud : public PointCloud<PointT> {
public:
using PointCloud<PointT>::points_;
using PointCloud<PointT>::width_;
using PointCloud<PointT>::height_;
using PointCloud<PointT>::IsOrganized;
using PointCloud<PointT>::sensor_to_world_pose_;
using PointCloud<PointT>::timestamp_;
// @brief default constructor
AttributePointCloud() = default;
// @brief copy constructor
AttributePointCloud(const AttributePointCloud<PointT>& pc) {
width_ = pc.width_;
height_ = pc.height_;
points_ = pc.points_;
points_timestamp_ = pc.points_timestamp_;
points_height_ = pc.points_height_;
points_beam_id_ = pc.points_beam_id_;
points_label_ = pc.points_label_;
}
// @brief construct from input point cloud and specified indices
AttributePointCloud(const AttributePointCloud<PointT>& pc,
const PointIndices& indices) {
CopyPointCloud(pc, indices);
}
AttributePointCloud(const AttributePointCloud<PointT>& pc,
const std::vector<int>& indices) {
CopyPointCloud(pc, indices);
}
// @brief construct given width and height for organized point cloud
AttributePointCloud(const size_t width, const size_t height,
const PointT point = PointT()) {
width_ = width;
height_ = height;
size_t size = width_ * height_;
points_.assign(size, point);
points_timestamp_.assign(size, 0.0);
points_height_.assign(size, std::numeric_limits<float>::max());
points_beam_id_.assign(size, -1);
points_label_.assign(size, 0);
}
// @brief destructor
virtual ~AttributePointCloud() = default;
// @brief add points of input cloud, return the self cloud
inline AttributePointCloud& operator+=(
const AttributePointCloud<PointT>& rhs) {
points_.insert(points_.end(), rhs.points_.begin(), rhs.points_.end());
points_timestamp_.insert(points_timestamp_.end(),
rhs.points_timestamp_.begin(),
rhs.points_timestamp_.end());
points_height_.insert(points_height_.end(), rhs.points_height_.begin(),
rhs.points_height_.end());
points_beam_id_.insert(points_beam_id_.end(), rhs.points_beam_id_.begin(),
rhs.points_beam_id_.end());
points_label_.insert(points_label_.end(), rhs.points_label_.begin(),
rhs.points_label_.end());
width_ = width_ * height_ + rhs.width_ * rhs.height_;
height_ = 1;
return *this;
}
// @brief overrided reserve function wrapper of vector
inline void reserve(const size_t size) override {
points_.reserve(size);
points_timestamp_.reserve(size);
points_height_.reserve(size);
points_beam_id_.reserve(size);
points_label_.reserve(size);
}
// @brief overrided resize function wrapper of vector
inline void resize(const size_t size) override {
points_.resize(size);
points_timestamp_.resize(size, 0.0);
points_height_.resize(size, std::numeric_limits<float>::max());
points_beam_id_.resize(size, -1);
points_label_.resize(size, 0);
if (size != width_ * height_) {
width_ = size;
height_ = 1;
}
}
// @brief overrided push_back function wrapper of vector
inline void push_back(const PointT& point) override {
points_.push_back(point);
points_timestamp_.push_back(0.0);
points_height_.push_back(std::numeric_limits<float>::max());
points_beam_id_.push_back(-1);
points_label_.push_back(0);
width_ = points_.size();
height_ = 1;
}
inline void push_back(const PointT& point, double timestamp,
float height = std::numeric_limits<float>::max(),
int32_t beam_id = -1, uint8_t label = 0) {
points_.push_back(point);
points_timestamp_.push_back(timestamp);
points_height_.push_back(height);
points_beam_id_.push_back(beam_id);
points_label_.push_back(label);
width_ = points_.size();
height_ = 1;
}
// @brief overrided clear function wrapper of vector
inline void clear() override {
points_.clear();
points_timestamp_.clear();
points_height_.clear();
points_beam_id_.clear();
points_label_.clear();
width_ = height_ = 0;
}
// @brief overrided swap point given source and target id
inline bool SwapPoint(const size_t source_id,
const size_t target_id) override {
if (source_id < points_.size() && target_id < points_.size()) {
std::swap(points_[source_id], points_[target_id]);
std::swap(points_timestamp_[source_id], points_timestamp_[target_id]);
std::swap(points_height_[source_id], points_height_[target_id]);
std::swap(points_beam_id_[source_id], points_beam_id_[target_id]);
std::swap(points_label_[source_id], points_label_[target_id]);
width_ = points_.size();
height_ = 1;
return true;
}
return false;
}
// @brief copy point from another point cloud
inline bool CopyPoint(const size_t id, const size_t rhs_id,
const AttributePointCloud<PointT>& rhs) {
if (id < points_.size() && rhs_id < rhs.points_.size()) {
points_[id] = rhs.points_[rhs_id];
points_timestamp_[id] = rhs.points_timestamp_[rhs_id];
points_height_[id] = rhs.points_height_[rhs_id];
points_beam_id_[id] = rhs.points_beam_id_[rhs_id];
points_label_[id] = rhs.points_label_[rhs_id];
return true;
}
return false;
}
// @brief copy point cloud
inline void CopyPointCloud(const AttributePointCloud<PointT>& rhs,
const PointIndices& indices) {
CopyPointCloud(rhs, indices.indices);
}
template <typename IndexType>
inline void CopyPointCloud(const AttributePointCloud<PointT>& rhs,
const std::vector<IndexType>& indices) {
width_ = indices.size();
height_ = 1;
points_.resize(indices.size());
points_timestamp_.resize(indices.size());
points_height_.resize(indices.size());
points_beam_id_.resize(indices.size());
points_label_.resize(indices.size());
for (size_t i = 0; i < indices.size(); ++i) {
points_[i] = rhs.points_[indices[i]];
points_timestamp_[i] = rhs.points_timestamp_[indices[i]];
points_height_[i] = rhs.points_height_[indices[i]];
points_beam_id_[i] = rhs.points_beam_id_[indices[i]];
points_label_[i] = rhs.points_label_[indices[i]];
}
}
// @brief swap point cloud
inline void SwapPointCloud(AttributePointCloud<PointT>* rhs) {
points_.swap(rhs->points_);
std::swap(width_, rhs->width_);
std::swap(height_, rhs->height_);
std::swap(sensor_to_world_pose_, rhs->sensor_to_world_pose_);
std::swap(timestamp_, rhs->timestamp_);
points_timestamp_.swap(rhs->points_timestamp_);
points_height_.swap(rhs->points_height_);
points_beam_id_.swap(rhs->points_beam_id_);
points_label_.swap(rhs->points_label_);
}
// @brief overrided check data member consistency
bool CheckConsistency() const override {
return ((points_.size() == points_timestamp_.size()) &&
(points_.size() == points_height_.size()) &&
(points_.size() == points_beam_id_.size()) &&
(points_.size() == points_label_.size()));
}
size_t TransferToIndex(const size_t col, const size_t row) const {
return row * width_ + col;
}
const std::vector<double>& points_timestamp() const {
return points_timestamp_;
}
std::vector<double>* mutable_points_timestamp() { return &points_timestamp_; }
const std::vector<float>& points_height() const { return points_height_; }
std::vector<float>* mutable_points_height() { return &points_height_; }
const std::vector<int32_t>& points_beam_id() const { return points_beam_id_; }
std::vector<int32_t>* mutable_points_beam_id() { return &points_beam_id_; }
const std::vector<uint8_t>& points_label() const { return points_label_; }
std::vector<uint8_t>* mutable_points_label() { return &points_label_; }
protected:
std::vector<double> points_timestamp_;
std::vector<float> points_height_;
std::vector<int32_t> points_beam_id_;
std::vector<uint8_t> points_label_;
};
} // namespace base
} // namespace perception
} // namespace apollo
#endif // MODULES_PERCEPTION_BASE_POINT_CLOUD_H_
modules/perception/base/point_cloud_test.cc 0 → 100644
浏览文件 @ 6ecf1e80
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* 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 "modules/perception/base/point_cloud_types.h"
#include <limits>
#include "gtest/gtest.h"
namespace apollo {
namespace perception {
namespace base {
TEST(PointTest, point_test) {
{
PointXYZIF point;
EXPECT_EQ(point.x, 0.f);
EXPECT_EQ(point.y, 0.f);
EXPECT_EQ(point.z, 0.f);
EXPECT_EQ(point.intensity, 0.f);
}
{
PointXYZID point;
EXPECT_EQ(point.x, 0.0);
EXPECT_EQ(point.y, 0.0);
EXPECT_EQ(point.z, 0.0);
EXPECT_EQ(point.intensity, 0.0);
}
{
PointXYZITF point;
EXPECT_EQ(point.x, 0.f);
EXPECT_EQ(point.timestamp, 0.0);
}
{
PointXYZITD point;
EXPECT_EQ(point.x, 0.0);
EXPECT_EQ(point.timestamp, 0.0);
}
{
PointXYZITHF point;
EXPECT_EQ(point.x, 0.f);
EXPECT_EQ(point.timestamp, 0.0);
EXPECT_EQ(point.height, std::numeric_limits<float>::max());
}
{
PointXYZITHD point;
EXPECT_EQ(point.x, 0.0);
EXPECT_EQ(point.timestamp, 0.0);
EXPECT_EQ(point.height, std::numeric_limits<float>::max());
}
{
PointXYZITHBF point;
EXPECT_EQ(point.x, 0.f);
EXPECT_EQ(point.timestamp, 0.0);
EXPECT_EQ(point.height, std::numeric_limits<float>::max());
EXPECT_EQ(point.beam_id, -1);
}
{
PointXYZITHBD point;
EXPECT_EQ(point.x, 0.0);
EXPECT_EQ(point.timestamp, 0.0);
EXPECT_EQ(point.height, std::numeric_limits<float>::max());
EXPECT_EQ(point.beam_id, -1);
}
{
PointXYZITHBLF point;
EXPECT_EQ(point.x, 0.f);
EXPECT_EQ(point.timestamp, 0.0);
EXPECT_EQ(point.height, std::numeric_limits<float>::max());
EXPECT_EQ(point.beam_id, -1);
EXPECT_EQ(point.label, 0);
}
{
PointXYZITHBLD point;
EXPECT_EQ(point.x, 0.0);
EXPECT_EQ(point.timestamp, 0.0);
EXPECT_EQ(point.height, std::numeric_limits<float>::max());
EXPECT_EQ(point.beam_id, -1);
EXPECT_EQ(point.label, 0);
}
}
TEST(PointIndicesTest, point_test) {
PointIndices indices;
EXPECT_EQ(indices.indices.capacity(), kDefaultReservePointNum);
}
TEST(PointCloudTest, point_cloud_constructor_test) {
{
using TestPointCloud = PointCloud<PointF>;
TestPointCloud cloud1;
EXPECT_EQ(cloud1.width(), 0);
EXPECT_EQ(cloud1.height(), 0);
EXPECT_EQ(cloud1.size(), 0);
EXPECT_TRUE(cloud1.empty());
TestPointCloud cloud2(cloud1);
EXPECT_EQ(cloud2.width(), 0);
EXPECT_EQ(cloud2.height(), 0);
EXPECT_EQ(cloud2.size(), 0);
cloud2.resize(10);
EXPECT_EQ(cloud2.width(), 10);
EXPECT_EQ(cloud2.height(), 1);
EXPECT_EQ(cloud2.size(), 10);
PointIndices indices;
indices.indices = {0, 3, 5};
EXPECT_EQ(indices.indices.size(), 3);
TestPointCloud cloud3(cloud2, indices);
EXPECT_EQ(cloud3.width(), 3);
EXPECT_EQ(cloud3.height(), 1);
TestPointCloud cloud4(cloud2, indices.indices);
EXPECT_EQ(cloud4.width(), 3);
EXPECT_EQ(cloud4.height(), 1);
TestPointCloud cloud5(6, 7);
EXPECT_TRUE(cloud5.IsOrganized());
EXPECT_EQ(cloud5.width(), 6);
EXPECT_EQ(cloud5.height(), 7);
cloud5 += cloud4;
EXPECT_EQ(cloud5.width(), 45);
EXPECT_EQ(cloud5.height(), 1);
}
{
using TestPointCloud = AttributePointCloud<PointF>;
TestPointCloud cloud1;
EXPECT_EQ(cloud1.width(), 0);
EXPECT_EQ(cloud1.height(), 0);
EXPECT_EQ(cloud1.size(), 0);
EXPECT_TRUE(cloud1.empty());
TestPointCloud cloud2(cloud1);
EXPECT_EQ(cloud2.width(), 0);
EXPECT_EQ(cloud2.height(), 0);
EXPECT_EQ(cloud2.size(), 0);
cloud2.resize(10);
EXPECT_EQ(cloud2.width(), 10);
EXPECT_EQ(cloud2.height(), 1);
EXPECT_EQ(cloud2.size(), 10);
PointIndices indices;
indices.indices = {0, 3, 5};
EXPECT_EQ(indices.indices.size(), 3);
TestPointCloud cloud3(cloud2, indices);
EXPECT_EQ(cloud3.width(), 3);
EXPECT_EQ(cloud3.height(), 1);
TestPointCloud cloud4(cloud2, indices.indices);
EXPECT_EQ(cloud4.width(), 3);
EXPECT_EQ(cloud4.height(), 1);
TestPointCloud cloud5(6, 7);
EXPECT_TRUE(cloud5.IsOrganized());
EXPECT_EQ(cloud5.width(), 6);
EXPECT_EQ(cloud5.height(), 7);
cloud5 += cloud4;
EXPECT_EQ(cloud5.width(), 45);
EXPECT_EQ(cloud5.height(), 1);
}
}
TEST(PointCloudTest, point_cloud_interface_test) {
typedef PointCloud<PointF> TestPointCloud;
TestPointCloud cloud;
cloud.reserve(2);
cloud.resize(2);
EXPECT_FALSE(cloud.IsOrganized());
TestPointCloud organized_cloud(2, 2);
organized_cloud.resize(4);
EXPECT_TRUE(organized_cloud.IsOrganized());
auto const_check_null = [](const TestPointCloud& cloud) {
EXPECT_EQ(cloud.at(0, 0), nullptr);
EXPECT_EQ(cloud(0, 0), nullptr);
};
auto const_check_not_null = [](const TestPointCloud& cloud) {
EXPECT_NE(cloud.at(0, 0), nullptr);
EXPECT_NE(cloud(0, 0), nullptr);
};
EXPECT_NE(organized_cloud.at(0, 0), nullptr);
EXPECT_NE(organized_cloud(0, 0), nullptr);
const_check_not_null(organized_cloud);
cloud.push_back(PointF());
EXPECT_EQ(cloud.at(0, 0), nullptr);
EXPECT_EQ(cloud(0, 0), nullptr);
const_check_null(cloud);
EXPECT_EQ(cloud.size(), 3);
auto const_check_eq = [](const TestPointCloud& cloud) {
EXPECT_EQ(&(cloud[0]), &(cloud.front()));
EXPECT_EQ(&(cloud[2]), &(cloud.back()));
};
EXPECT_EQ(&(cloud[0]), &(cloud.front()));
EXPECT_EQ(&(cloud[2]), &(cloud.back()));
const_check_eq(cloud);
for (auto it = cloud.mutable_points()->begin();
it != cloud.mutable_points()->end(); ++it) {
EXPECT_EQ(it->x, 0.f);
}
for (auto it = cloud.points().begin(); it != cloud.points().end(); ++it) {
EXPECT_EQ(it->x, 0.f);
}
cloud.clear();
EXPECT_EQ(cloud.size(), 0);
cloud.resize(1);
EXPECT_FALSE(cloud.SwapPoint(0, 3));
EXPECT_FALSE(cloud.SwapPoint(3, 0));
cloud.resize(2);
cloud[0].x = 1.f;
cloud[1].x = 2.f;
EXPECT_TRUE(cloud.SwapPoint(0, 1));
EXPECT_EQ(cloud[0].x, 2.f);
EXPECT_EQ(cloud[1].x, 1.f);
TestPointCloud cloud2;
cloud2.resize(1);
cloud2[0].x = 10.f;
EXPECT_FALSE(cloud.CopyPoint(0, 10, cloud2));
EXPECT_TRUE(cloud.CopyPoint(0, 0, cloud2));
EXPECT_EQ(cloud[0].x, 10.f);
cloud.resize(1);
cloud.set_timestamp(10.0);
Eigen::Affine3d pose = Eigen::Affine3d::Identity();
pose.translate(Eigen::Vector3d(20.0, 0, 0));
cloud.set_sensor_to_world_pose(pose);
cloud2.clear();
cloud2.SwapPointCloud(&cloud);
EXPECT_EQ(cloud2.size(), 1);
EXPECT_NEAR(cloud2.get_timestamp(), 10.0, 1e-9);
EXPECT_NEAR(cloud2.sensor_to_world_pose().translation()(0), 20.0, 1e-9);
}
TEST(PointCloudTest, attribute_point_cloud_interface_test) {
using TestPointCloud = AttributePointCloud<PointF>;
TestPointCloud cloud;
cloud.reserve(2);
cloud.resize(2);
cloud.push_back(PointF());
cloud.push_back(PointF(), 1.0, 0.5f, 1, 2);
auto const_check_eq = [](const TestPointCloud& cloud) {
EXPECT_EQ(cloud.points_timestamp().at(3), 1.0);
EXPECT_EQ(cloud.points_height().at(3), 0.5f);
EXPECT_EQ(cloud.points_beam_id().at(3), 1);
EXPECT_EQ(cloud.TransferToIndex(0, 0), 0);
};
EXPECT_EQ(cloud.points_timestamp().at(3), 1.0);
EXPECT_EQ(cloud.points_height().at(3), 0.5f);
EXPECT_EQ(cloud.points_beam_id().at(3), 1);
EXPECT_EQ(cloud.points_label().at(3), 2);
const_check_eq(cloud);
TestPointCloud organized_cloud(2, 2);
organized_cloud.reserve(4);
organized_cloud.resize(4);
EXPECT_TRUE(organized_cloud.IsOrganized());
organized_cloud.mutable_points_timestamp()->at(3) = 1.0;
organized_cloud.mutable_points_height()->at(3) = 0.5f;
organized_cloud.mutable_points_beam_id()->at(3) = 1;
organized_cloud.mutable_points_label()->at(3) = 2;
auto const_check_eq_organized = [](const TestPointCloud& cloud) {
EXPECT_EQ(cloud.points_timestamp().at(3), 1.0);
EXPECT_EQ(cloud.points_height().at(3), 0.5f);
EXPECT_EQ(cloud.points_beam_id().at(3), 1);
EXPECT_EQ(cloud.points_label().at(3), 2);
EXPECT_EQ(cloud.TransferToIndex(0, 0), 0);
};
EXPECT_EQ(organized_cloud.points_timestamp().at(3), 1.0);
EXPECT_EQ(organized_cloud.points_height().at(3), 0.5f);
EXPECT_EQ(organized_cloud.points_beam_id().at(3), 1);
EXPECT_EQ(organized_cloud.points_label().at(3), 2);
const_check_eq_organized(organized_cloud);
cloud.clear();
EXPECT_EQ(cloud.size(), 0);
cloud.resize(1);
EXPECT_FALSE(cloud.SwapPoint(0, 3));
EXPECT_FALSE(cloud.SwapPoint(3, 0));
cloud.resize(2);
cloud[0].x = 1.f;
cloud.mutable_points_timestamp()->at(0) = 0.0;
cloud.mutable_points_height()->at(0) = 0.f;
cloud.mutable_points_beam_id()->at(0) = 0;
cloud.mutable_points_label()->at(0) = 0;
cloud[1].x = 2.f;
cloud.mutable_points_timestamp()->at(1) = 1.0;
cloud.mutable_points_height()->at(1) = 1.f;
cloud.mutable_points_beam_id()->at(1) = 1;
cloud.mutable_points_label()->at(1) = 1;
EXPECT_TRUE(cloud.SwapPoint(0, 1));
EXPECT_EQ(cloud[0].x, 2.f);
EXPECT_EQ(cloud.points_timestamp().at(0), 1.0);
EXPECT_EQ(cloud.points_height().at(0), 1.f);
EXPECT_EQ(cloud.points_beam_id().at(0), 1);
EXPECT_EQ(cloud.points_label().at(0), 1);
EXPECT_EQ(cloud[1].x, 1.f);
EXPECT_EQ(cloud.points_timestamp().at(1), 0.0);
EXPECT_EQ(cloud.points_height().at(1), 0.f);
EXPECT_EQ(cloud.points_beam_id().at(1), 0);
EXPECT_EQ(cloud.points_label().at(1), 0);
TestPointCloud cloud2;
cloud2.resize(1);
cloud2[0].x = 10.f;
cloud2.mutable_points_timestamp()->at(0) = 10.0;
cloud2.mutable_points_height()->at(0) = 10.f;
cloud2.mutable_points_beam_id()->at(0) = 10;
cloud2.mutable_points_label()->at(0) = 10;
EXPECT_FALSE(cloud.CopyPoint(0, 10, cloud2));
EXPECT_TRUE(cloud.CopyPoint(0, 0, cloud2));
EXPECT_EQ(cloud[0].x, 10.f);
EXPECT_EQ(cloud.points_timestamp().at(0), 10.0);
EXPECT_EQ(cloud.points_height().at(0), 10.f);
EXPECT_EQ(cloud.points_beam_id().at(0), 10);
EXPECT_EQ(cloud.points_label().at(0), 10);
cloud.resize(1);
cloud.set_timestamp(10.0);
Eigen::Affine3d pose = Eigen::Affine3d::Identity();
pose.translate(Eigen::Vector3d(20.0, 0, 0));
cloud.set_sensor_to_world_pose(pose);
cloud.resize(2);
cloud.mutable_points_timestamp()->at(1) = 2.0;
cloud.mutable_points_height()->at(1) = 3.f;
cloud.mutable_points_beam_id()->at(1) = 4;
cloud.mutable_points_label()->at(1) = 5;
cloud2.clear();
cloud2.SwapPointCloud(&cloud);
EXPECT_EQ(cloud2.size(), 2);
EXPECT_NEAR(cloud2.get_timestamp(), 10.0, 1e-9);
EXPECT_NEAR(cloud2.sensor_to_world_pose().translation()(0), 20.0, 1e-9);
EXPECT_NEAR(cloud2.points_timestamp().at(1), 2.0, 1e-9);
EXPECT_NEAR(cloud2.points_height().at(1), 3.f, 1e-9);
EXPECT_EQ(cloud2.points_beam_id().at(1), 4);
EXPECT_EQ(cloud2.points_label().at(1), 5);
}
TEST(PointCloudTest, transform_test) {
Eigen::Affine3d affine = Eigen::Affine3d::Identity();
affine.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(1, 0, 0)));
affine.translate(Eigen::Vector3d(1, 1, 1));
typedef AttributePointCloud<PointF> TestPointCloud;
TestPointCloud cloud(4, 4);
cloud.set_timestamp(1.0);
EXPECT_EQ(cloud.get_timestamp(), 1.0);
cloud.set_sensor_to_world_pose(affine);
EXPECT_EQ((cloud.sensor_to_world_pose().matrix() - affine.matrix()).trace(),
0.0);
cloud.RotatePointCloud(false);
EXPECT_EQ(cloud.sensor_to_world_pose().linear().trace(), 3.0);
cloud.TransformPointCloud(false);
EXPECT_EQ(cloud.sensor_to_world_pose().translation().x(), 0.0);
EXPECT_EQ(cloud.sensor_to_world_pose().translation().y(), 0.0);
EXPECT_EQ(cloud.sensor_to_world_pose().translation().z(), 0.0);
EXPECT_EQ(cloud.sensor_to_world_pose().matrix().determinant(), 1.0);
cloud[0].x = std::numeric_limits<float>::quiet_NaN();
cloud[1].y = std::numeric_limits<float>::quiet_NaN();
cloud[2].z = std::numeric_limits<float>::quiet_NaN();
cloud.RotatePointCloud(true);
EXPECT_EQ(cloud.sensor_to_world_pose().linear().trace(), 3.0);
cloud.TransformPointCloud(true);
EXPECT_EQ(cloud.sensor_to_world_pose().translation().x(), 0.0);
EXPECT_EQ(cloud.sensor_to_world_pose().translation().y(), 0.0);
EXPECT_EQ(cloud.sensor_to_world_pose().translation().z(), 0.0);
EXPECT_EQ(cloud.sensor_to_world_pose().matrix().determinant(), 1.0);
}
template <typename PointT>
void CloudCheck(const std::shared_ptr<const PointCloud<PointT>> cloud) {
for (const auto& point : cloud->points()) {
EXPECT_EQ(point.x, 0.f);
EXPECT_EQ(point.y, 0.f);
EXPECT_EQ(point.z, 0.f);
}
}
template <typename PointT>
void ResizeCloud(const std::shared_ptr<PointCloud<PointT>> cloud) {
cloud->resize(cloud->size() * 2);
}
TEST(PointCloudTest, dynamic_binding_test) {
std::shared_ptr<PointCloud<PointF>> cloud;
cloud.reset(new PointCloud<PointF>);
cloud->resize(10);
CloudCheck<PointF>(cloud);
EXPECT_TRUE(cloud->CheckConsistency());
std::shared_ptr<AttributePointCloud<PointF>> attribute_cloud;
attribute_cloud.reset(new AttributePointCloud<PointF>);
attribute_cloud->resize(10);
CloudCheck<PointF>(attribute_cloud);
ResizeCloud<PointF>(attribute_cloud);
EXPECT_EQ(attribute_cloud->size(), 20);
EXPECT_TRUE(attribute_cloud->CheckConsistency());
}
} // namespace base
} // namespace perception
} // namespace apollo
modules/perception/base/point_cloud_types.h 0 → 100644
浏览文件 @ 6ecf1e80
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* 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.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_BASE_POINT_CLOUD_TYPES_H_
#define MODULES_PERCEPTION_BASE_POINT_CLOUD_TYPES_H_
#include <memory>
#include "modules/perception/base/point_cloud.h"
namespace apollo {
namespace perception {
namespace base {
// alias of Point<float> and Point<double>
using PointF = PointXYZIF;
using PointD = PointXYZID;
// point cloud
using PointFCloud = AttributePointCloud<PointXYZIF>;
using PointDCloud = AttributePointCloud<PointXYZID>;
// polygon
using PolygonFType = PointCloud<PointF>;
using PolygonDType = PointCloud<PointD>;
} // namespace base
} // namespace perception
} // namespace apollo
#endif // MODULES_PERCEPTION_BASE_POINT_CLOUD_TYPES_H_
modules/perception/common/geometry/BUILD 0 → 100644
浏览文件 @ 6ecf1e80
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "basic",
hdrs = [
"basic.h",
],
)
# cc_test(
# name = "disjoint_set_test",
# size = "small",
# srcs = [
# "disjoint_set_test.cc",
# ],
# deps = [
# ":disjoint_set",
# "@gtest//:main",
# ],
# )
cpplint()
modules/perception/common/geometry/basic.h 0 → 100644
浏览文件 @ 6ecf1e80
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* 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.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_COMMON_GEOMETRY_BASIC_H_
#define MODULES_PERCEPTION_COMMON_GEOMETRY_BASIC_H_
#include <algorithm>
#include <limits>
#include <vector>
#include "Eigen/Core"
#include "modules/perception/base/point_cloud_types.h"
namespace apollo {
namespace perception {
namespace common {
// @brief cross production on two vectors
// one is from pt1 to pt2, another is from pt1 to pt3
// the type of points could be double or float
// old name: cross_prod
template <typename Type>
inline Type CrossProduct(const Eigen::Matrix<Type, 2, 1> &point1,
const Eigen::Matrix<Type, 2, 1> &point2,
const Eigen::Matrix<Type, 2, 1> &point3) {
return (point2.x() - point1.x()) * (point3.y() - point1.y()) -
(point3.x() - point1.x()) * (point2.y() - point1.y());
}
// @brief cross production on two vectors
// one is from pt1 to pt2, another is from pt1 to pt3
// the type of points could be double or float
// old name: cross_prod
template <typename PointT>
inline typename PointT::Type CrossProduct(const PointT &point1,
const PointT &point2,
const PointT &point3) {
return (point2.x - point1.x) * (point3.y - point1.y) -
(point3.x - point1.x) * (point2.y - point1.y);
}
// @brief calculate the Eucliden distance between two points
// old name: euclidean_dist
template <typename PointT>
inline typename PointT::Type CalculateEuclidenDist(const PointT &pt1,
const PointT &pt2) {
typename PointT::Type dist = (pt1.x - pt2.x) * (pt1.x - pt2.x);
dist += (pt1.y - pt2.y) * (pt1.y - pt2.y);
dist += (pt1.z - pt2.z) * (pt1.z - pt2.z);
return sqrt(dist);
}
// @brief calculate the Eucliden distance between two points in X-Y plane
// old name: euclidean_dist_2d_xy
template <typename PointT>
inline typename PointT::T CalculateEuclidenDist2DXY(const PointT &pt1,
const PointT &pt2) {
typename PointT::T dist = (pt1.x - pt2.x) * (pt1.x - pt2.x);
dist += (pt1.y - pt2.y) * (pt1.y - pt2.y);
return sqrt(dist);
}
// @brief calculate cos value of the rotation angle
// between two vectors in X-Y plane
// old name: vector_cos_theta_2d_xy
template <typename T>
T CalculateCosTheta2DXY(const Eigen::Matrix<T, 3, 1> &v1,
const Eigen::Matrix<T, 3, 1> &v2) {
T v1_len = sqrt((v1.head(2).cwiseProduct(v1.head(2))).sum());
T v2_len = sqrt((v2.head(2).cwiseProduct(v2.head(2))).sum());
if (v1_len < std::numeric_limits<T>::epsilon() ||
v2_len < std::numeric_limits<T>::epsilon()) {
return 0.0;
}
T cos_theta = (v1.head(2).cwiseProduct(v2.head(2))).sum() / (v1_len * v2_len);
return cos_theta;
}
// @brief calculate the rotation angle between two vectors in X-Y plane
// old name: vector_theta_2d_xy
template <typename T>
T CalculateTheta2DXY(const Eigen::Matrix<T, 3, 1> &v1,
const Eigen::Matrix<T, 3, 1> &v2) {
T v1_len = sqrt((v1.head(2).cwiseProduct(v1.head(2))).sum());
T v2_len = sqrt((v2.head(2).cwiseProduct(v2.head(2))).sum());
if (v1_len < std::numeric_limits<T>::epsilon() ||
v2_len < std::numeric_limits<T>::epsilon()) {
return 0.0;
}
const T cos_theta =
(v1.head(2).cwiseProduct(v2.head(2))).sum() / (v1_len * v2_len);
const T sin_theta = (v1(0) * v2(1) - v1(1) * v2(0)) / (v1_len * v2_len);
T theta = std::acos(cos_theta);
if (sin_theta < 0.0) {
theta = -theta;
}
return theta;
}
// @brief calculate the rotation matrix
// transform from v1 axis coordinate to v2 axis coordinate
// old name: vector_rot_mat_2d_xy
template <typename T>
Eigen::Matrix<T, 3, 3> CalculateRotationMat2DXY(
const Eigen::Matrix<T, 3, 1> &v1, const Eigen::Matrix<T, 3, 1> &v2) {
T v1_len = sqrt((v1.head(2).cwiseProduct(v1.head(2))).sum());
T v2_len = sqrt((v2.head(2).cwiseProduct(v2.head(2))).sum());
if (v1_len < std::numeric_limits<T>::epsilon() ||
v2_len < std::numeric_limits<T>::epsilon()) {
return Eigen::Matrix<T, 3, 3>::Zero(3, 3);
}
const T cos_theta =
(v1.head(2).cwiseProduct(v2.head(2))).sum() / (v1_len * v2_len);
const T sin_theta = (v1(0) * v2(1) - v1(1) * v2(0)) / (v1_len * v2_len);
Eigen::Matrix<T, 3, 3> rot_mat;
rot_mat << cos_theta, sin_theta, 0, -sin_theta, cos_theta, 0, 0, 0, 1;
return rot_mat;
}
// @brief calculate the project vector from one vector to another
// old name: compute_2d_xy_project_vector
template <typename T>
Eigen::Matrix<T, 3, 1> Calculate2DXYProjectVector(
const Eigen::Matrix<T, 3, 1> &projected_vector,
const Eigen::Matrix<T, 3, 1> &project_vector) {
if (projected_vector.head(2).norm() < std::numeric_limits<T>::epsilon() ||
project_vector.head(2).norm() < std::numeric_limits<T>::epsilon()) {
return Eigen::Matrix<T, 3, 1>::Zero(3, 1);
}
Eigen::Matrix<T, 3, 1> project_dir = project_vector;
project_dir(2) = 0.0;
project_dir.normalize();
const T projected_vector_project_dir_inner_product =
projected_vector(0) * project_dir(0) +
projected_vector(1) * project_dir(1);
const T projected_vector_project_dir_angle_cos =
projected_vector_project_dir_inner_product /
(projected_vector.head(2).norm() * project_dir.head(2).norm());
const T projected_vector_norm_on_project_dir =
projected_vector.head(2).norm() * projected_vector_project_dir_angle_cos;
return project_dir * projected_vector_norm_on_project_dir;
}
// @brief convert point xyz in Cartesian coordinate to polar coordinate
// old name: xyz_to_polar_coordinate
template <typename PointT>
void ConvertCartesiantoPolarCoordinate(const PointT &xyz,
typename PointT::Type *h_angle_in_degree,
typename PointT::Type *v_angle_in_degree,
typename PointT::Type *dist) {
using T = typename PointT::Type;
const T radius_to_degree = 180.0 / M_PI;
const T x = xyz.x;
const T y = xyz.y;
const T z = xyz.z;
(*dist) = sqrt(x * x + y * y + z * z);
T dist_xy = sqrt(x * x + y * y);
(*h_angle_in_degree) = std::acos(x / dist_xy) * radius_to_degree;
if (y < 0.0) {
(*h_angle_in_degree) = 360.0 - (*h_angle_in_degree);
}
(*v_angle_in_degree) = std::acos(dist_xy / (*dist)) * radius_to_degree;
if (z < 0.0) {
(*v_angle_in_degree) = -(*v_angle_in_degree);
}
}
} // namespace common
} // namespace perception
} // namespace apollo
#endif // MODULES_PERCEPTION_COMMON_GEOMETRY_BASIC_H_
modules/perception/common/graph/BUILD
浏览文件 @ 6ecf1e80
......@@ -119,4 +119,29 @@ cc_test(
],
)
cc_library(
name = "conditional_clustering",
hdrs = [
"conditional_clustering.h",
],
deps = [
"//modules/perception/base:point_cloud",
"//modules/perception/common/geometry:basic",
"@eigen",
],
)
cc_test(
name = "conditional_clustering_test",
size = "small",
srcs = [
"conditional_clustering_test.cc",
],
deps = [
":conditional_clustering",
"@gtest//:main",
],
)
cpplint()
modules/perception/common/graph/conditional_clustering.h 0 → 100644
浏览文件 @ 6ecf1e80
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* 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.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_COMMON_GEOMETRY_CONDITIONAL_CLUSTERING_H_
#define MODULES_PERCEPTION_COMMON_GEOMETRY_CONDITIONAL_CLUSTERING_H_
#include <limits>
#include <memory>
#include <vector>
#include "modules/perception/base/point.h"
#include "modules/perception/base/point_cloud.h"
#include "modules/perception/base/point_cloud_types.h"
namespace apollo {
namespace perception {
namespace common {
// @brief: define of the conditional clustering class
template <typename PointT>
class ConditionClustering {
public:
using IndicesClusters = std::vector<base::PointIndices>;
ConditionClustering() = default;
explicit ConditionClustering(bool extract_removed_clusters = false)
: min_cluster_size_(1),
max_cluster_size_(std::numeric_limits<int>::max()),
extract_removed_clusters_(extract_removed_clusters),
small_clusters_(new IndicesClusters),
large_clusters_(new IndicesClusters) {}
virtual ~ConditionClustering() = default;
inline void set_cloud(
typename std::shared_ptr<base::PointCloud<PointT>> cloud) {
cloud_ = cloud;
}
inline void set_candidate_param(void* param) { candidate_param_ = param; }
inline void set_candidate_function(
int (*candidate_function)(const PointT&, void*, std::vector<int>*)) {
candidate_function_ = candidate_function;
}
inline void set_condition_param(void* param) { condition_param_ = param; }
inline void set_condition_function(bool (*condition_function)(const PointT&,
const PointT&,
void* param)) {
condition_function_ = condition_function;
}
inline void set_min_cluster_size(int size) { min_cluster_size_ = size; }
inline void set_max_cluster_size(int size) { max_cluster_size_ = size; }
// main interface of ConditionClustering class to segment.
void Segment(IndicesClusters* xy_clusters);
private:
typename std::shared_ptr<base::PointCloud<PointT>> cloud_;
int min_cluster_size_ = 0;
int max_cluster_size_ = 0;
bool extract_removed_clusters_ = true;
std::shared_ptr<IndicesClusters> small_clusters_;
std::shared_ptr<IndicesClusters> large_clusters_;
bool (*condition_function_)(const PointT&, const PointT&,
void* param) = nullptr;
int (*candidate_function_)(const PointT&, void*,
std::vector<int>* nn_indices_ptr) = nullptr;
void* candidate_param_ = nullptr;
void* condition_param_ = nullptr;
}; // class ConditionClustering
template <typename PointT>
void ConditionClustering<PointT>::Segment(IndicesClusters* xy_clusters) {
std::vector<int> nn_indices;
nn_indices.reserve(200);
std::vector<bool> processed(cloud_->size(), false);
// Process all points indexed by indices_
for (std::size_t iii = 0; iii < cloud_->size(); ++iii) {
if (processed[iii]) {
continue;
}
// Set up a new growing cluster
std::vector<int> current_cluster;
current_cluster.reserve(200);
std::size_t cii = 0;
current_cluster.push_back(iii);
processed[iii] = true;
// Process the current cluster
// (it can be growing in size as it is being processed)
while (cii < current_cluster.size()) {
nn_indices.clear();
if (candidate_function_(cloud_->at(current_cluster[cii]),
candidate_param_, &nn_indices) < 1) {
cii++;
continue;
}
for (std::size_t nii = 1; nii < nn_indices.size(); ++nii) {
if (nn_indices[nii] < 0 || nn_indices[nii] >= processed.size() ||
processed[nn_indices[nii]]) {
continue;
}
if (condition_function_(cloud_->at(current_cluster[cii]),
cloud_->at(nn_indices[nii]),
condition_param_)) {
current_cluster.push_back(nn_indices[nii]);
processed[nn_indices[nii]] = true;
}
}
cii++;
}
if (extract_removed_clusters_ ||
(current_cluster.size() >= min_cluster_size_ &&
current_cluster.size() <= max_cluster_size_)) {
base::PointIndices pi;
pi.indices.resize(current_cluster.size());
for (int ii = 0; ii < static_cast<int>(current_cluster.size()); ++ii) {
pi.indices[ii] = current_cluster[ii];
}
if (extract_removed_clusters_ &&
current_cluster.size() < min_cluster_size_) {
small_clusters_->push_back(pi);
} else if (extract_removed_clusters_ &&
current_cluster.size() > max_cluster_size_) {
large_clusters_->push_back(pi);
} else {
xy_clusters->push_back(pi);
}
}
}
}
} // namespace common
} // namespace perception
} // namespace apollo
#endif // MODULES_PERCEPTION_COMMON_GEOMETRY_CONDITIONAL_CLUSTERING_H_
modules/perception/common/graph/conditional_clustering_test.cc 0 → 100644
浏览文件 @ 6ecf1e80
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* 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 "modules/perception/common/graph/conditional_clustering.h"
#include <limits>
#include "gtest/gtest.h"
#include "modules/perception/base/point_cloud_types.h"
#include "modules/perception/common/geometry/basic.h"
namespace apollo {
namespace perception {
namespace common {
namespace {
int EasyCandidate(const base::PointF& p, void* vg,
std::vector<int>* nn_indices) {
nn_indices->push_back(1);
return nn_indices->size();
}
int EasyCandidateAll(const base::PointF& p, void* vg,
std::vector<int>* nn_indices) {
for (int i = 0; i < 300; ++i) {
nn_indices->push_back(i);
}
return nn_indices->size();
}
int EasyCandidateError(const base::PointF& p, void* vg,
std::vector<int>* nn_indices) {
return -1;
}
bool EasyCondition(const base::PointF& p1, const base::PointF& p2, void* vg) {
float dist = 5.0f;
if (CalculateEuclidenDist(p1, p2) <= dist) {
return true;
}
return false;
}
bool EasyConditionFalse(const base::PointF& p1, const base::PointF& p2,
void* vg) {
return false;
}
} // namespace
using IndicesClusters = std::vector<base::PointIndices>;
TEST(ConditionalClusteringTest, conditional_clustering_test) {
ConditionClustering<base::PointF> condition_clustering =
ConditionClustering<base::PointF>(false);
std::shared_ptr<base::PolygonFType> polygon_in_ptr =
std::shared_ptr<base::PolygonFType>(
new base::PolygonFType(16, 16, base::PointF()));
base::PolygonFType polygon_out;
base::PointF tmp_pt;
size_t i, j;
for (i = 0; i < 8; ++i) {
for (j = 0; j < 8; ++j) {
tmp_pt.x = 0.5f * i;
tmp_pt.y = 0.5f * j;
tmp_pt.z = 0;
*(polygon_in_ptr->at(i, j)) = tmp_pt;
}
}
for (i = 0; i < 16; ++i) {
for (j = 0; j < 16; ++j) {
tmp_pt.x = 0.5f * i + 100;
tmp_pt.y = 0.5f * j + 100;
tmp_pt.z = 0;
*(polygon_in_ptr->at(i, j)) = tmp_pt;
}
}
condition_clustering.set_cloud(polygon_in_ptr);
condition_clustering.set_candidate_function(EasyCandidate);
condition_clustering.set_condition_function(EasyCondition);
condition_clustering.set_min_cluster_size(1);
condition_clustering.set_max_cluster_size(10);
IndicesClusters indices_clusters;
EXPECT_EQ(indices_clusters.size(), 0);
condition_clustering.Segment(&indices_clusters);
EXPECT_EQ(indices_clusters.size(), 256);
indices_clusters.resize(0);
condition_clustering.set_min_cluster_size(100);
condition_clustering.set_max_cluster_size(1000);
condition_clustering.Segment(&indices_clusters);
EXPECT_EQ(indices_clusters.size(), 0);
indices_clusters.resize(0);
condition_clustering.set_candidate_function(EasyCandidateAll);
condition_clustering.set_min_cluster_size(1);
condition_clustering.set_max_cluster_size(256);
condition_clustering.Segment(&indices_clusters);
EXPECT_EQ(indices_clusters.size(), 1);
indices_clusters.resize(0);
indices_clusters.resize(0);
condition_clustering.set_candidate_function(EasyCandidateError);
condition_clustering.Segment(&indices_clusters);
EXPECT_EQ(indices_clusters.size(), 256);
indices_clusters.resize(0);
condition_clustering.set_candidate_function(EasyCandidate);
condition_clustering.set_condition_function(EasyConditionFalse);
condition_clustering.set_min_cluster_size(1);
condition_clustering.set_max_cluster_size(256);
condition_clustering.Segment(&indices_clusters);
EXPECT_EQ(indices_clusters.size(), 256);
}
TEST(ConditionalClusteringTest, conditional_clustering_test1) {
ConditionClustering<base::PointF> condition_clustering =
ConditionClustering<base::PointF>(true);
std::shared_ptr<base::PolygonFType> polygon_in_ptr =
std::shared_ptr<base::PolygonFType>(
new base::PolygonFType(16, 16, base::PointF()));
base::PolygonFType polygon_out;
base::PointF tmp_pt;
size_t i, j;
for (i = 0; i < 8; ++i) {
for (j = 0; j < 8; ++j) {
tmp_pt.x = 0.5f * i;
tmp_pt.y = 0.5f * j;
tmp_pt.z = 0;
*(polygon_in_ptr->at(i, j)) = tmp_pt;
}
}
for (i = 0; i < 16; ++i) {
for (j = 0; j < 16; ++j) {
tmp_pt.x = 0.5f * i + 100;
tmp_pt.y = 0.5f * j + 100;
tmp_pt.z = 0;
*(polygon_in_ptr->at(i, j)) = tmp_pt;
}
}
condition_clustering.set_cloud(polygon_in_ptr);
condition_clustering.set_candidate_function(EasyCandidate);
condition_clustering.set_condition_function(EasyCondition);
condition_clustering.set_min_cluster_size(1);
condition_clustering.set_max_cluster_size(10);
IndicesClusters indices_clusters;
EXPECT_EQ(indices_clusters.size(), 0);
condition_clustering.Segment(&indices_clusters);
EXPECT_EQ(indices_clusters.size(), 256);
indices_clusters.resize(0);
condition_clustering.set_min_cluster_size(100);
condition_clustering.set_max_cluster_size(1000);
condition_clustering.Segment(&indices_clusters);
EXPECT_EQ(indices_clusters.size(), 0);
indices_clusters.resize(0);
condition_clustering.set_candidate_function(EasyCandidateAll);
condition_clustering.set_min_cluster_size(1);
condition_clustering.set_max_cluster_size(256);
condition_clustering.Segment(&indices_clusters);
EXPECT_EQ(indices_clusters.size(), 1);
indices_clusters.resize(0);
indices_clusters.resize(0);
condition_clustering.set_candidate_function(EasyCandidateError);
condition_clustering.Segment(&indices_clusters);
EXPECT_EQ(indices_clusters.size(), 256);
indices_clusters.resize(0);
condition_clustering.set_candidate_function(EasyCandidate);
condition_clustering.set_condition_function(EasyConditionFalse);
condition_clustering.set_min_cluster_size(1);
condition_clustering.set_max_cluster_size(256);
condition_clustering.Segment(&indices_clusters);
EXPECT_EQ(indices_clusters.size(), 256);
}
} // namespace common
} // namespace perception
} // namespace apollo
modules/perception/common/graph/graph_segmentor.cc
浏览文件 @ 6ecf1e80
......@@ -17,6 +17,7 @@
#include "modules/perception/common/graph/graph_segmentor.h"
#include <cfloat>
#include <limits>
#include "cybertron/common/log.h"
......@@ -33,7 +34,7 @@ void GraphSegmentor::Init(const float initial_threshold) {
thresholds_.reserve(kMaxVerticesNum);
thresholds_table_.resize(kMaxThresholdsNum);
thresholds_table_[0] = FLT_MAX;
thresholds_table_[0] = std::numeric_limits<float>::max();
for (size_t i = 1; i < kMaxThresholdsNum; ++i) {
thresholds_table_[i] = GetThreshold(i, initial_threshold_);
}
......
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或