Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

前往新版Gitcode，体验更适合开发者的 AI 搜索 >>

未验证 提交 7461c419 编写于 作者: Y Yifei Jiang 提交者： GitHub
浏览文件
操作

Merge branch 'v3.1_dev' into r2.5.0

上级 c748a604 621de127
展开全部 隐藏空白更改
内联 并排
Showing with 2212 addition and 851 deletion
modules/localization/lmd/BUILD
浏览文件 @ 7461c419
......@@ -18,7 +18,13 @@ cc_library(
"//modules/common/proto:common_proto",
"//modules/common/status",
"//modules/common/time",
"//modules/common/util:thread_pool",
"//modules/localization:localization_base",
"//modules/localization/common:localization_common",
"//modules/localization/lmd/predictor:localization_lmd_predictor",
"//modules/localization/lmd/predictor/output:localization_lmd_predictor_output",
"//modules/localization/lmd/predictor/raw:localization_lmd_predictor_raw_gps",
"//modules/localization/lmd/predictor/raw:localization_lmd_predictor_raw_imu",
"//modules/localization/proto:localization_config_proto",
"//modules/localization/proto:localization_proto",
"//modules/localization/proto:odometry_lane_marker_proto",
......@@ -35,10 +41,10 @@ cc_test(
],
data = ["//modules/localization:localization_testdata"],
deps = [
":lmd_localization",
"//modules/common:log",
"//modules/common/time",
"//modules/common/util",
"//modules/localization/lmd:lmd_localization",
"@gtest//:main",
],
)
......
modules/localization/lmd/common/BUILD 0 → 100644
浏览文件 @ 7461c419
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "localization_lmd_common",
srcs = [
"pose_list.cc",
],
hdrs = [
"pose_list.h",
"tm_list.h",
],
deps = [
"//modules/common:log",
"//modules/common/math",
"//modules/localization/proto:localization_proto",
],
)
cc_test(
name = "localization_lmd_common_test",
size = "small",
srcs = [
"pose_list_test.cc",
"tm_list_test.cc",
],
deps = [
":localization_lmd_common",
"@gtest//:main",
],
)
cpplint()
modules/localization/lmd/common/pose_list.cc 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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/localization/lmd/common/pose_list.h"
#include <cmath>
#include "modules/common/log.h"
#include "modules/common/math/quaternion.h"
namespace apollo {
namespace localization {
using apollo::common::math::QuaternionToHeading;
namespace {
template <class T>
T InterpolateXYZ(const T &p1, const T &p2, double frac1) {
T p;
double frac2 = 1.0 - frac1;
if (p1.has_x() && !std::isnan(p1.x()) && p2.has_x() && !std::isnan(p2.x())) {
p.set_x(p1.x() * frac2 + p2.x() * frac1);
}
if (p1.has_y() && !std::isnan(p1.y()) && p2.has_y() && !std::isnan(p2.y())) {
p.set_y(p1.y() * frac2 + p2.y() * frac1);
}
if (p1.has_z() && !std::isnan(p1.z()) && p2.has_z() && !std::isnan(p2.z())) {
p.set_z(p1.z() * frac2 + p2.z() * frac1);
}
return p;
}
template <class T>
T InterpolateQuaternion(const T &p1, const T &p2, double frac1) {
T p;
T p21 = p2;
auto cosa = p1.qw() * p2.qw() + p1.qx() * p2.qx() + p1.qy() * p2.qy() +
p1.qz() * p2.qz();
if (cosa < 0.0) {
p21.set_qw(-p2.qw());
p21.set_qx(-p2.qx());
p21.set_qy(-p2.qy());
p21.set_qz(-p2.qz());
cosa = -cosa;
}
double frac2;
constexpr double kCriticalCosa = 0.9995;
if (cosa > kCriticalCosa) {
// if cosa near 1.0, user linear interpolation
frac2 = 1.0 - frac1;
} else {
auto sina = std::sqrt(1.0 - cosa * cosa);
auto a = std::atan2(sina, cosa);
frac2 = std::sin((1.0 - frac1) * a) / sina;
frac1 = std::sin(frac1 * a) / sina;
}
p.set_qw(p1.qw() * frac2 + p21.qw() * frac1);
p.set_qx(p1.qx() * frac2 + p21.qx() * frac1);
p.set_qy(p1.qy() * frac2 + p21.qy() * frac1);
p.set_qz(p1.qz() * frac2 + p21.qz() * frac1);
return p;
}
} // namespace
bool PoseList::FindMatchingPose(double timestamp_sec, Pose *pose) const {
CHECK_NOTNULL(pose);
auto p = RangeOf(timestamp_sec);
if (p.first == end() || p.second == end()) {
return false;
}
InterpolatePose(p.first->first, p.first->second, p.second->first,
p.second->second, timestamp_sec, pose);
return true;
}
bool PoseList::FindNearestPose(double timestamp_sec, Pose *pose) const {
CHECK_NOTNULL(pose);
if (!FindMatchingPose(timestamp_sec, pose)) {
auto it = Nearest(timestamp_sec);
if (it == end()) {
return false;
}
pose->CopyFrom(it->second);
}
return true;
}
void PoseList::InterpolatePose(double timestamp_sec1, const Pose &pose1,
double timestamp_sec2, const Pose &pose2,
double timestamp_sec, Pose *pose) {
CHECK_GE(timestamp_sec, timestamp_sec1);
CHECK_GE(timestamp_sec2, timestamp_sec);
CHECK_NOTNULL(pose);
auto time_diff = timestamp_sec2 - timestamp_sec1;
if (fabs(time_diff) > 0.0) {
auto frac1 = (timestamp_sec - timestamp_sec1) / time_diff;
if (pose1.has_position() && pose2.has_position()) {
auto val = InterpolateXYZ(pose1.position(), pose2.position(), frac1);
pose->mutable_position()->CopyFrom(val);
}
if (pose1.has_orientation() && pose2.has_orientation() &&
pose1.orientation().has_qw() && pose1.orientation().has_qx() &&
pose1.orientation().has_qy() && pose1.orientation().has_qz() &&
pose2.orientation().has_qw() && pose2.orientation().has_qx() &&
pose2.orientation().has_qy() && pose2.orientation().has_qz()) {
auto val = InterpolateQuaternion(pose1.orientation(), pose2.orientation(),
frac1);
pose->mutable_orientation()->CopyFrom(val);
pose->set_heading(QuaternionToHeading(
pose->orientation().qw(), pose->orientation().qx(),
pose->orientation().qy(), pose->orientation().qz()));
}
if (pose1.has_linear_velocity() && pose2.has_linear_velocity()) {
auto val = InterpolateXYZ(pose1.linear_velocity(),
pose2.linear_velocity(), frac1);
pose->mutable_linear_velocity()->CopyFrom(val);
}
if (pose1.has_angular_velocity() && pose2.has_angular_velocity()) {
auto val = InterpolateXYZ(pose1.angular_velocity(),
pose2.angular_velocity(), frac1);
pose->mutable_angular_velocity()->CopyFrom(val);
}
if (pose1.has_linear_acceleration() && pose2.has_linear_acceleration()) {
auto val = InterpolateXYZ(pose1.linear_acceleration(),
pose2.linear_acceleration(), frac1);
pose->mutable_linear_acceleration()->CopyFrom(val);
}
if (pose1.has_euler_angles() && pose2.has_euler_angles()) {
auto val =
InterpolateXYZ(pose1.euler_angles(), pose2.euler_angles(), frac1);
pose->mutable_euler_angles()->CopyFrom(val);
}
}
}
} // namespace localization
} // namespace apollo
modules/localization/lmd/common/pose_list.h 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file pose_list.h
* @brief The class of PoseList.
*/
#ifndef MODULES_LOCALIZATION_LMD_COMMON_POSE_LIST_H_
#define MODULES_LOCALIZATION_LMD_COMMON_POSE_LIST_H_
#include "modules/localization/proto/localization.pb.h"
#include "modules/localization/lmd/common/tm_list.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace apollo {
namespace localization {
/**
* @class PoseList
*
* @brief A time marked list of poses.
*/
class PoseList : public TimeMarkedList<Pose> {
public:
explicit PoseList(double memory_cycle_sec = 0.0)
: TimeMarkedList<Pose>(memory_cycle_sec) {}
/**
* @brief Get a pose by timestamp, may need interpolation.
* @param timestamp_sec The required timestamp.
* @param pose The output.
* @return True if found.
*/
bool FindMatchingPose(double timestamp_sec, Pose *pose) const;
/**
* @brief Get a nearest pose by timestamp, may need interpolation.
* @param timestamp_sec The required timestamp.
* @param pose The output.
* @return True if found.
*/
bool FindNearestPose(double timestamp_sec, Pose *pose) const;
/**
* @brief Get a pose by interpolating two poses.
* @param timestamp_sec1 The timestamp of pose 1.
* @param pose1 The pose 1.
* @param timestamp_sec2 The timestamp of pose 2.
* @param pose2 The pose 2.
* @param timestamp_sec The required timestamp, must >=timestamp_sec1 and
* <=timestamp_sec2.
* @param pose The output.
*/
static void InterpolatePose(double timestamp_sec1, const Pose &pose1,
double timestamp_sec2, const Pose &pose2,
double timestamp_sec, Pose *pose);
};
} // namespace localization
} // namespace apollo
#endif // MODULES_LOCALIZATION_LMD_COMMON_POSE_LIST_H_
modules/localization/lmd/common/pose_list_test.cc 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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/localization/lmd/common/pose_list.h"
#include "gtest/gtest.h"
namespace apollo {
namespace localization {
TEST(PoseListTest, FindMatchingPose) {
Pose pose1;
pose1.mutable_position()->set_x(0.0);
pose1.mutable_position()->set_y(0.0);
pose1.mutable_position()->set_z(0.0);
pose1.mutable_orientation()->set_qw(1.0);
pose1.mutable_orientation()->set_qx(0.0);
pose1.mutable_orientation()->set_qy(0.0);
pose1.mutable_orientation()->set_qz(0.0);
pose1.mutable_linear_velocity()->set_x(0.0);
pose1.mutable_linear_velocity()->set_y(0.0);
pose1.mutable_linear_velocity()->set_z(0.0);
pose1.mutable_angular_velocity()->set_x(0.0);
pose1.mutable_angular_velocity()->set_y(0.0);
pose1.mutable_angular_velocity()->set_z(0.0);
pose1.mutable_linear_acceleration()->set_x(0.0);
pose1.mutable_linear_acceleration()->set_y(0.0);
pose1.mutable_linear_acceleration()->set_z(0.0);
pose1.mutable_euler_angles()->set_x(0.0);
pose1.mutable_euler_angles()->set_y(0.0);
pose1.mutable_euler_angles()->set_z(0.0);
Pose pose2;
pose2.mutable_position()->set_x(1.0);
pose2.mutable_position()->set_y(1.0);
pose2.mutable_position()->set_z(1.0);
pose2.mutable_orientation()->set_qw(0.707);
pose2.mutable_orientation()->set_qx(0.0);
pose2.mutable_orientation()->set_qy(0.0);
pose2.mutable_orientation()->set_qz(0.707);
pose2.mutable_linear_velocity()->set_x(1.0);
pose2.mutable_linear_velocity()->set_y(1.0);
pose2.mutable_linear_velocity()->set_z(1.0);
pose2.mutable_angular_velocity()->set_x(1.0);
pose2.mutable_angular_velocity()->set_y(1.0);
pose2.mutable_angular_velocity()->set_z(1.0);
pose2.mutable_linear_acceleration()->set_x(1.0);
pose2.mutable_linear_acceleration()->set_y(1.0);
pose2.mutable_linear_acceleration()->set_z(1.0);
pose2.mutable_euler_angles()->set_x(1.0);
pose2.mutable_euler_angles()->set_y(1.0);
pose2.mutable_euler_angles()->set_z(1.0);
PoseList pose_list(100.0);
pose_list.Push(0.0, pose1);
pose_list.Push(1.0, pose2);
Pose pose;
EXPECT_TRUE(pose_list.FindMatchingPose(0.333, &pose));
EXPECT_NEAR(0.333, pose.position().x(), 1e-10);
EXPECT_NEAR(0.333, pose.position().y(), 1e-10);
EXPECT_NEAR(0.333, pose.position().z(), 1e-10);
EXPECT_NEAR(0.965981, pose.orientation().qw(), 1e-6);
EXPECT_NEAR(0.0, pose.orientation().qx(), 1e-6);
EXPECT_NEAR(0.0, pose.orientation().qy(), 1e-6);
EXPECT_NEAR(0.258537, pose.orientation().qz(), 1e-6);
EXPECT_NEAR(0.333, pose.linear_velocity().x(), 1e-10);
EXPECT_NEAR(0.333, pose.linear_velocity().y(), 1e-10);
EXPECT_NEAR(0.333, pose.linear_velocity().z(), 1e-10);
EXPECT_NEAR(0.333, pose.angular_velocity().x(), 1e-10);
EXPECT_NEAR(0.333, pose.angular_velocity().y(), 1e-10);
EXPECT_NEAR(0.333, pose.angular_velocity().z(), 1e-10);
EXPECT_NEAR(0.333, pose.linear_acceleration().x(), 1e-10);
EXPECT_NEAR(0.333, pose.linear_acceleration().y(), 1e-10);
EXPECT_NEAR(0.333, pose.linear_acceleration().z(), 1e-10);
EXPECT_NEAR(0.333, pose.euler_angles().x(), 1e-10);
EXPECT_NEAR(0.333, pose.euler_angles().y(), 1e-10);
EXPECT_NEAR(0.333, pose.euler_angles().z(), 1e-10);
EXPECT_TRUE(pose_list.FindMatchingPose(1e-11, &pose));
EXPECT_NEAR(0.0, pose.position().x(), 1e-10);
EXPECT_NEAR(0.0, pose.position().y(), 1e-10);
EXPECT_NEAR(0.0, pose.position().z(), 1e-10);
EXPECT_TRUE(pose_list.FindMatchingPose(1.0, &pose));
EXPECT_NEAR(1.0, pose.position().x(), 1e-10);
EXPECT_NEAR(1.0, pose.position().y(), 1e-10);
EXPECT_NEAR(1.0, pose.position().z(), 1e-10);
EXPECT_FALSE(pose_list.FindMatchingPose(-1.0, &pose));
EXPECT_FALSE(pose_list.FindMatchingPose(2.0, &pose));
}
TEST(PoseListTest, FindNearestPose) {
PoseList pose_list(100.0);
Pose pose;
EXPECT_FALSE(pose_list.FindNearestPose(0.0, &pose));
Pose pose1;
pose1.mutable_position()->set_x(0.0);
pose1.mutable_position()->set_y(0.0);
pose1.mutable_position()->set_z(0.0);
pose_list.Push(10.0, pose1);
Pose pose2;
pose2.mutable_position()->set_x(1.0);
pose2.mutable_position()->set_y(1.0);
pose2.mutable_position()->set_z(1.0);
pose_list.Push(11.0, pose2);
EXPECT_TRUE(pose_list.FindNearestPose(9.0, &pose));
EXPECT_NEAR(0.0, pose.position().x(), 1e-10);
EXPECT_NEAR(0.0, pose.position().y(), 1e-10);
EXPECT_NEAR(0.0, pose.position().z(), 1e-10);
EXPECT_TRUE(pose_list.FindNearestPose(14.0, &pose));
EXPECT_NEAR(1.0, pose.position().x(), 1e-10);
EXPECT_NEAR(1.0, pose.position().y(), 1e-10);
EXPECT_NEAR(1.0, pose.position().z(), 1e-10);
EXPECT_TRUE(pose_list.FindNearestPose(10.5, &pose));
EXPECT_NEAR(0.5, pose.position().x(), 1e-10);
EXPECT_NEAR(0.5, pose.position().y(), 1e-10);
EXPECT_NEAR(0.5, pose.position().z(), 1e-10);
}
} // namespace localization
} // namespace apollo
modules/localization/lmd/common/tm_list.h 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file tm_list.h
* @brief The class of TimeMarkedList.
*/
#ifndef MODULES_LOCALIZATION_LMD_COMMON_TM_LIST_H_
#define MODULES_LOCALIZATION_LMD_COMMON_TM_LIST_H_
#include <algorithm>
#include <map>
#include <utility>
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace apollo {
namespace localization {
/**
* @class TimeMarkedList
*
* @brief A time marked list.
*/
template <class Data>
class TimeMarkedList : public std::map<double, Data> {
public:
typedef typename std::map<double, Data>::const_iterator Iterator;
explicit TimeMarkedList(double memory_cycle_sec = 0.0) {
memory_cycle_sec_ = std::max(0.0, memory_cycle_sec);
}
double MemoryCycleSec() const { return memory_cycle_sec_; }
bool Push(double timestamp_sec, const Data& data) {
return Push(timestamp_sec, Data(data));
}
bool Push(double timestamp_sec, Data&& data) {
auto latest = Latest();
if (latest != std::map<double, Data>::end() &&
latest->first >= timestamp_sec) {
return false;
}
std::map<double, Data>::emplace(timestamp_sec, std::move(data));
auto oldest = Oldest();
if (oldest != std::map<double, Data>::end()) {
auto oldest1 = oldest;
oldest1++;
if (oldest1 != std::map<double, Data>::end() &&
timestamp_sec - oldest->first > memory_cycle_sec_ &&
timestamp_sec - oldest1->first >= memory_cycle_sec_) {
std::map<double, Data>::erase(oldest);
}
}
return true;
}
Iterator Latest() const {
if (!std::map<double, Data>::empty()) {
auto iter = std::map<double, Data>::end();
iter--;
return iter;
} else {
return std::map<double, Data>::end();
}
}
Iterator Oldest() const { return std::map<double, Data>::begin(); }
template <class T>
bool Older(const TimeMarkedList<T>& other) const {
auto it = other.Latest();
if (it != other.end()) {
return Older(it->first);
}
return false;
}
bool Older(double timestamp_sec) const {
auto it = Latest();
if (it != std::map<double, Data>::end()) {
return it->first < timestamp_sec;
}
return false;
}
template <class T>
bool Newer(const TimeMarkedList<T>& other) const {
auto it = other.Latest();
if (it != other.end()) {
return Newer(it->first);
}
return false;
}
bool Newer(double timestamp_sec) const {
auto it = Latest();
if (it != std::map<double, Data>::end()) {
return it->first > timestamp_sec;
}
return false;
}
std::pair<Iterator, Iterator> RangeOf(double timestamp_sec) const {
auto upper = std::map<double, Data>::lower_bound(timestamp_sec);
Iterator lower;
if (upper != std::map<double, Data>::end()) {
if (upper == std::map<double, Data>::begin()) {
lower = std::map<double, Data>::end();
} else {
lower = upper;
lower--;
}
} else {
if (!std::map<double, Data>::empty()) {
lower = std::map<double, Data>::end();
lower--;
} else {
lower = std::map<double, Data>::end();
}
}
return std::make_pair(lower, upper);
}
Iterator Nearest(double timestamp_sec) const {
auto p = RangeOf(timestamp_sec);
if (p.first == std::map<double, Data>::end() &&
p.second == std::map<double, Data>::end()) {
return std::map<double, Data>::end();
}
if (p.first == std::map<double, Data>::end()) {
return p.second;
} else if (p.second == std::map<double, Data>::end()) {
return p.first;
}
if (timestamp_sec - p.first->first < p.second->first - timestamp_sec) {
return p.first;
} else {
return p.second;
}
}
private:
double memory_cycle_sec_;
};
} // namespace localization
} // namespace apollo
#endif // MODULES_LOCALIZATION_LMD_COMMON_TM_LIST_H_
modules/localization/lmd/common/tm_list_test.cc 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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/localization/lmd/common/tm_list.h"
#include "gtest/gtest.h"
namespace apollo {
namespace localization {
TEST(TimeMarkedListTest, InsertAndFind) {
TimeMarkedList<int> tm(100.0);
for (auto i = 0; i < 200; ++i) {
tm.Push(static_cast<double>(i), i);
}
EXPECT_EQ(101, tm.size());
auto latest = tm.Latest();
EXPECT_NE(tm.end(), latest);
if (latest != tm.end()) {
EXPECT_EQ(199, latest->second);
}
auto oldest = tm.Oldest();
EXPECT_NE(tm.end(), oldest);
if (oldest != tm.end()) {
EXPECT_EQ(99, oldest->second);
}
}
TEST(TimeMarkedListTest, Older) {
TimeMarkedList<int> tm(100.0);
for (auto i = 0; i < 20; ++i) {
tm.Push(static_cast<double>(i), i);
}
TimeMarkedList<int> tm1(100.0);
for (auto i = 0; i < 30; ++i) {
tm1.Push(static_cast<double>(i), i);
}
TimeMarkedList<int> tm2(100.0);
EXPECT_TRUE(tm.Older(tm1));
EXPECT_FALSE(tm1.Older(tm));
EXPECT_FALSE(tm.Older(tm2));
EXPECT_FALSE(tm2.Older(tm1));
}
TEST(TimeMarkedListTest, Newer) {
TimeMarkedList<int> tm(100.0);
for (auto i = 0; i < 20; ++i) {
tm.Push(static_cast<double>(i), i);
}
TimeMarkedList<int> tm1(100.0);
for (auto i = 0; i < 30; ++i) {
tm1.Push(static_cast<double>(i), i);
}
TimeMarkedList<int> tm2(100.0);
EXPECT_TRUE(tm1.Newer(tm));
EXPECT_FALSE(tm.Newer(tm1));
EXPECT_FALSE(tm2.Newer(tm));
EXPECT_FALSE(tm1.Newer(tm2));
}
TEST(TimeMarkedListTest, RangeOf) {
TimeMarkedList<double> tm(100.0);
auto p = tm.RangeOf(10.0);
EXPECT_EQ(tm.end(), p.first);
EXPECT_EQ(tm.end(), p.second);
for (double i = 5.0; i <= 20.0; i += 1.0) {
tm.Push(i, i);
}
p = tm.RangeOf(10.5);
EXPECT_NE(tm.end(), p.first);
EXPECT_NE(tm.end(), p.second);
EXPECT_NEAR(10.0, p.first->first, 1e-10);
EXPECT_NEAR(11.0, p.second->first, 1e-10);
p = tm.RangeOf(3.0);
EXPECT_EQ(tm.end(), p.first);
EXPECT_NE(tm.end(), p.second);
EXPECT_NEAR(5.0, p.second->first, 1e-10);
p = tm.RangeOf(30.0);
EXPECT_NE(tm.end(), p.first);
EXPECT_EQ(tm.end(), p.second);
EXPECT_NEAR(20.0, p.first->first, 1e-10);
p = tm.RangeOf(5.0);
EXPECT_EQ(tm.end(), p.first);
EXPECT_NE(tm.end(), p.second);
EXPECT_NEAR(5.0, p.second->first, 1e-10);
}
TEST(TimeMarkedListTest, Nearest) {
TimeMarkedList<double> tm(100.0);
auto it = tm.Nearest(10.0);
EXPECT_EQ(tm.end(), it);
for (auto i = 5.0; i <= 20.0; i += 1.0) {
tm.Push(i, i);
}
it = tm.Nearest(3.0);
EXPECT_NE(tm.end(), it);
EXPECT_NEAR(5.0, it->first, 1e-10);
it = tm.Nearest(21.0);
EXPECT_NE(tm.end(), it);
EXPECT_NEAR(20.0, it->first, 1e-10);
it = tm.Nearest(15.7);
EXPECT_NE(tm.end(), it);
EXPECT_NEAR(16.0, it->first, 1e-10);
}
} // namespace localization
} // namespace apollo
modules/localization/lmd/lmd_localization.h
浏览文件 @ 7461c419
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
* 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.
......@@ -22,24 +22,26 @@
#ifndef MODULES_LOCALIZATION_LMD_LMD_LOCALIZATION_H_
#define MODULES_LOCALIZATION_LMD_LMD_LOCALIZATION_H_
#include <algorithm>
#include <sstream>
#include <chrono>
#include <future>
#include <list>
#include <map>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "ros/include/ros/ros.h"
#include "modules/canbus/proto/chassis.pb.h"
#include "modules/common/monitor_log/monitor_log_buffer.h"
#include "modules/common/status/status.h"
#include "modules/localization/lmd/predictor/predictor.h"
#include "modules/localization/localization_base.h"
#include "modules/localization/proto/gps.pb.h"
#include "modules/localization/proto/imu.pb.h"
#include "modules/localization/proto/localization.pb.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/common/monitor_log/monitor_log_buffer.h"
#include "modules/common/status/status.h"
#include "modules/localization/localization_base.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
......@@ -53,6 +55,23 @@ namespace localization {
* @brief Generate localization info based on LMD.
*/
class LMDLocalization : public LocalizationBase {
struct PredictorHandler {
std::shared_ptr<Predictor> predictor;
std::future<apollo::common::Status> fut;
PredictorHandler() = default;
explicit PredictorHandler(Predictor *predictor) {
this->predictor.reset(predictor);
}
bool Busy() const {
if (!fut.valid()) return false;
auto s = fut.wait_for(std::chrono::seconds::zero());
return s != std::future_status::ready;
}
};
public:
LMDLocalization();
virtual ~LMDLocalization();
......@@ -70,32 +89,28 @@ class LMDLocalization : public LocalizationBase {
apollo::common::Status Stop() override;
private:
void OnImu(const CorrectedImu &imu);
void OnGps(const Gps &gps);
void OnChassis(const apollo::canbus::Chassis &chassis);
void OnPerceptionObstacles(
const apollo::perception::PerceptionObstacles &obstacles);
void OnTimer(const ros::TimerEvent &event);
void PrepareLocalizationMsg(LocalizationEstimate *localization);
bool GetGpsPose(const Gps &gps, Pose *pose, double *timestamp_sec);
bool PredictPose(const Pose &old_pose, double old_timestamp_sec,
double new_timestamp_sec, Pose *new_pose);
bool FindMatchingGPS(double timestamp_sec, Gps *gps_msg);
bool FindMatchingIMU(double timestamp_sec, CorrectedImu *imu_msg);
bool FindMatchingChassis(double timestamp_sec,
apollo::canbus::Chassis *chassis_msg);
bool PredictByLinearIntergrate(const Pose &old_pose, double old_timestamp_sec,
double new_timestamp_sec, Pose *new_pose);
void PrintPoseError(const Pose &pose, double timestamp_sec);
void Predicting();
void RunWatchDog();
private:
ros::Timer timer_;
apollo::common::monitor::MonitorLogger monitor_logger_;
const std::vector<double> map_offset_;
bool has_last_pose_ = false;
Pose last_pose_;
double last_pose_timestamp_sec_;
std::map<std::string, PredictorHandler> predictors_;
PredictorHandler *gps_;
PredictorHandler *imu_;
PredictorHandler *output_;
std::list<CorrectedImu> imu_list_;
std::list<Gps> gps_list_;
std::list<apollo::perception::PerceptionObstacles> obstacles_list_;
};
} // namespace localization
......
modules/localization/lmd/lmd_localization_test.cc
浏览文件 @ 7461c419
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
* 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.
......
modules/localization/lmd/predictor/BUILD 0 → 100644
浏览文件 @ 7461c419
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "localization_lmd_predictor",
hdrs = [
"predictor.h",
],
deps = [
"//modules/common/status",
"//modules/localization/lmd/common:localization_lmd_common",
],
)
cpplint()
modules/localization/lmd/predictor/output/BUILD 0 → 100644
浏览文件 @ 7461c419
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "localization_lmd_predictor_output",
srcs = [
"predictor_output.cc",
"predictor_print_error.cc",
],
hdrs = [
"predictor_output.h",
"predictor_print_error.h",
],
deps = [
"//modules/common:log",
"//modules/common/math",
"//modules/common/proto:common_proto",
"//modules/common/status",
"//modules/localization/common:localization_common",
"//modules/localization/lmd/common:localization_lmd_common",
"//modules/localization/lmd/predictor:localization_lmd_predictor",
"//modules/localization/proto:localization_proto",
"@gtest",
],
)
cc_test(
name = "localization_lmd_predictor_output_test",
size = "small",
srcs = [
"predictor_output_test.cc"
],
deps = [
":localization_lmd_predictor_output",
"@gtest//:main",
],
)
cpplint()
modules/localization/lmd/predictor/output/predictor_output.cc 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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/localization/lmd/predictor/output/predictor_output.h"
#include <cmath>
#include "modules/common/proto/geometry.pb.h"
#include "modules/common/log.h"
#include "modules/common/math/euler_angles_zxy.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/math/quaternion.h"
#include "modules/localization/common/localization_gflags.h"
namespace apollo {
namespace localization {
using apollo::common::Point3D;
using apollo::common::PointENU;
using apollo::common::Quaternion;
using apollo::common::Status;
using apollo::common::math::EulerAnglesZXYd;
using apollo::common::math::QuaternionRotate;
using apollo::common::math::QuaternionToHeading;
namespace {
template <class T>
T QuaternionRotateXYZ(const T v, const Quaternion& orientation) {
auto vec =
QuaternionRotate(orientation, Eigen::Vector3d(v.x(), v.y(), v.z()));
T ret;
ret.set_x(vec[0]);
ret.set_y(vec[1]);
ret.set_z(vec[2]);
return ret;
}
void FillPoseFromImu(const Pose& imu_pose, Pose* pose) {
// linear acceleration
if (imu_pose.has_linear_acceleration()) {
if (FLAGS_enable_map_reference_unify) {
if (pose->has_orientation()) {
pose->mutable_linear_acceleration()->CopyFrom(QuaternionRotateXYZ(
imu_pose.linear_acceleration(), pose->orientation()));
pose->mutable_linear_acceleration_vrf()->CopyFrom(
imu_pose.linear_acceleration());
} else {
AERROR << "Fail to convert linear_acceleration";
}
} else {
pose->mutable_linear_acceleration()->CopyFrom(
imu_pose.linear_acceleration());
}
}
// angular velocity
if (imu_pose.has_angular_velocity()) {
if (FLAGS_enable_map_reference_unify) {
if (FLAGS_enable_map_reference_unify) {
if (pose->has_orientation()) {
pose->mutable_angular_velocity()->CopyFrom(QuaternionRotateXYZ(
imu_pose.angular_velocity(), pose->orientation()));
pose->mutable_angular_velocity_vrf()->CopyFrom(
imu_pose.angular_velocity());
} else {
AERROR << "Fail to convert angular_velocity";
}
} else {
pose->mutable_angular_velocity()->CopyFrom(imu_pose.angular_velocity());
}
} else {
pose->mutable_angular_velocity()->CopyFrom(imu_pose.angular_velocity());
}
}
// euler angle
if (imu_pose.has_euler_angles())
pose->mutable_euler_angles()->CopyFrom(imu_pose.euler_angles());
}
} // namespace
PredictorOutput::PredictorOutput(
double memory_cycle_sec,
const std::function<Status(double, const Pose&)>& publish_loc_func)
: Predictor(memory_cycle_sec), publish_loc_func_(publish_loc_func) {
name_ = kPredictorOutputName;
dep_predicteds_.emplace(kPredictorGpsName, PoseList(memory_cycle_sec));
dep_predicteds_.emplace(kPredictorImuName, PoseList(memory_cycle_sec));
on_adapter_thread_ = true;
}
PredictorOutput::~PredictorOutput() {}
bool PredictorOutput::Updateable() const {
const auto& imu = dep_predicteds_.find(kPredictorImuName)->second;
if (predicted_.empty()) {
const auto& gps = dep_predicteds_.find(kPredictorGpsName)->second;
return !gps.empty() && !imu.empty();
} else {
return !imu.empty() && predicted_.Older(imu);
}
}
Status PredictorOutput::Update() {
if (predicted_.empty()) {
const auto& gps = dep_predicteds_[kPredictorGpsName];
auto gps_latest = gps.Latest();
auto timestamp_sec = gps_latest->first;
auto pose = gps_latest->second;
if (!pose.has_heading() && pose.has_orientation()) {
pose.set_heading(QuaternionToHeading(
pose.orientation().qw(), pose.orientation().qx(),
pose.orientation().qy(), pose.orientation().qz()));
}
const auto& imu = dep_predicteds_[kPredictorImuName];
Pose imu_pose;
imu.FindNearestPose(timestamp_sec, &imu_pose);
// fill pose from imu
FillPoseFromImu(imu_pose, &pose);
// push pose to list
predicted_.Push(timestamp_sec, pose);
// publish
return publish_loc_func_(timestamp_sec, pose);
} else {
// get timestamp from imu
const auto& imu = dep_predicteds_[kPredictorImuName];
auto timestamp_sec = imu.Latest()->first;
// base pose for prediction
auto base_timestamp_sec = predicted_.Latest()->first;
auto base_pose = predicted_.Latest()->second;
// predict
Pose pose;
PredictByImu(base_timestamp_sec, base_pose, timestamp_sec, &pose);
// push pose to list
predicted_.Push(timestamp_sec, pose);
// publish
return publish_loc_func_(timestamp_sec, pose);
}
}
bool PredictorOutput::PredictByImu(double old_timestamp_sec,
const Pose& old_pose,
double new_timestamp_sec, Pose* new_pose) {
if (!old_pose.has_position() || !old_pose.has_orientation() ||
!old_pose.has_linear_velocity()) {
AERROR << "Pose has no some fields";
return false;
}
const auto& imu = dep_predicteds_[kPredictorImuName];
auto p = imu.RangeOf(old_timestamp_sec);
auto it = p.first;
if (it == imu.end()) {
if (p.second != imu.end()) {
it = p.second;
} else {
AERROR << std::setprecision(15)
<< "Cannot get the lower of range from imu with timestamp["
<< old_timestamp_sec << "]";
return false;
}
}
auto timestamp_sec = old_timestamp_sec;
new_pose->CopyFrom(old_pose);
bool finished = false;
while (!finished) {
Pose imu_pose;
double timestamp_sec_1;
Pose imu_pose_1;
auto it_1 = it;
it_1++;
if (it_1 != imu.end() && new_timestamp_sec >= it_1->first) {
PoseList::InterpolatePose(it->first, it->second, it_1->first,
it_1->second, timestamp_sec, &imu_pose);
timestamp_sec_1 = it_1->first;
imu_pose_1 = it_1->second;
} else {
timestamp_sec_1 = new_timestamp_sec;
imu_pose = it->second;
imu_pose_1 = imu_pose;
}
if (new_timestamp_sec <= timestamp_sec_1) {
finished = true;
}
if (!finished && timestamp_sec_1 <= timestamp_sec) {
it = it_1;
imu_pose = imu_pose_1;
continue;
}
if (!imu_pose.has_linear_acceleration() ||
!imu_pose_1.has_linear_acceleration() ||
!imu_pose.has_angular_velocity() ||
!imu_pose_1.has_angular_velocity()) {
AERROR << "Imu_pose or imu_pose_1 has no some fields";
return false;
}
auto dt = timestamp_sec_1 - timestamp_sec;
auto orientation = new_pose->orientation();
Point3D angular_velocity;
if (FLAGS_enable_map_reference_unify) {
angular_velocity.CopyFrom(
QuaternionRotateXYZ(imu_pose.angular_velocity(), orientation));
} else {
angular_velocity.CopyFrom(imu_pose.angular_velocity());
}
Point3D angular_velocity_1;
if (FLAGS_enable_map_reference_unify) {
angular_velocity_1.CopyFrom(
QuaternionRotateXYZ(imu_pose_1.angular_velocity(), orientation));
} else {
angular_velocity_1.CopyFrom(imu_pose_1.angular_velocity());
}
Point3D angular_vel;
angular_vel.set_x((angular_velocity.x() + angular_velocity_1.x()) / 2.0);
angular_vel.set_y((angular_velocity.y() + angular_velocity_1.y()) / 2.0);
angular_vel.set_z((angular_velocity.z() + angular_velocity_1.z()) / 2.0);
EulerAnglesZXYd euler_a(orientation.qw(), orientation.qx(),
orientation.qy(), orientation.qz());
auto derivation_roll =
angular_vel.x() +
std::sin(euler_a.roll()) * std::tan(euler_a.pitch()) * angular_vel.y() +
std::cos(euler_a.roll()) * std::tan(euler_a.pitch()) * angular_vel.z();
auto derivation_pitch = std::cos(euler_a.roll()) * angular_vel.y() -
std::sin(euler_a.roll()) * angular_vel.z();
auto derivation_yaw =
std::sin(euler_a.roll()) / std::cos(euler_a.pitch()) * angular_vel.y() +
std::cos(euler_a.roll()) / std::cos(euler_a.pitch()) * angular_vel.z();
EulerAnglesZXYd euler_b(euler_a.roll() + derivation_roll * dt,
euler_a.pitch() + derivation_pitch * dt,
euler_a.yaw() + derivation_yaw * dt);
auto q = euler_b.ToQuaternion();
Quaternion orientation_1;
orientation_1.set_qw(q.w());
orientation_1.set_qx(q.x());
orientation_1.set_qy(q.y());
orientation_1.set_qz(q.z());
Point3D linear_acceleration;
if (FLAGS_enable_map_reference_unify) {
linear_acceleration.CopyFrom(
QuaternionRotateXYZ(imu_pose.linear_acceleration(), orientation));
} else {
linear_acceleration.CopyFrom(imu_pose.linear_acceleration());
}
Point3D linear_acceleration_1;
if (FLAGS_enable_map_reference_unify) {
linear_acceleration_1.CopyFrom(
QuaternionRotateXYZ(imu_pose_1.linear_acceleration(), orientation_1));
} else {
linear_acceleration_1.CopyFrom(imu_pose_1.linear_acceleration());
}
auto linear_velocity = new_pose->linear_velocity();
Point3D linear_velocity_1;
linear_velocity_1.set_x(
(linear_acceleration.x() + linear_acceleration_1.x()) / 2.0 * dt +
linear_velocity.x());
linear_velocity_1.set_y(
(linear_acceleration.y() + linear_acceleration_1.y()) / 2.0 * dt +
linear_velocity.y());
linear_velocity_1.set_z(
(linear_acceleration.z() + linear_acceleration_1.z()) / 2.0 * dt +
linear_velocity.z());
auto position = new_pose->position();
PointENU position_1;
position_1.set_x(
(linear_acceleration.x() / 3.0 + linear_acceleration_1.x() / 6.0) * dt *
dt +
linear_velocity.x() * dt + position.x());
position_1.set_y(
(linear_acceleration.y() / 3.0 + linear_acceleration_1.y() / 6.0) * dt *
dt +
linear_velocity.y() * dt + position.y());
position_1.set_z(
(linear_acceleration.z() / 3.0 + linear_acceleration_1.z() / 6.0) * dt *
dt +
linear_velocity.z() * dt + position.z());
new_pose->mutable_position()->CopyFrom(position_1);
new_pose->mutable_orientation()->CopyFrom(orientation_1);
new_pose->set_heading(QuaternionToHeading(
new_pose->orientation().qw(), new_pose->orientation().qx(),
new_pose->orientation().qy(), new_pose->orientation().qz()));
new_pose->mutable_linear_velocity()->CopyFrom(linear_velocity_1);
FillPoseFromImu(imu_pose_1, new_pose);
if (!finished) {
timestamp_sec = timestamp_sec_1;
it = it_1;
imu_pose = imu_pose_1;
}
}
return true;
}
} // namespace localization
} // namespace apollo
modules/localization/lmd/predictor/output/predictor_output.h 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file predictor_output.h
* @brief The class of PredictorOutput.
*/
#ifndef MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_OUTPUT_H_
#define MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_OUTPUT_H_
#include <functional>
#include "gtest/gtest.h"
#include "modules/localization/lmd/predictor/predictor.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace apollo {
namespace localization {
/**
* @class PredictorOutput
*
* @brief Implementation of predictor.
*/
class PredictorOutput : public Predictor {
public:
explicit PredictorOutput(
double memory_cycle_sec,
const std::function<apollo::common::Status(double, const Pose &)>
&publish_loc_func);
virtual ~PredictorOutput();
/**
* @brief Overrided implementation of the virtual function "Updateable" in the
* base class "Predictor".
* @return True if yes; no otherwise.
*/
bool Updateable() const override;
/**
* @brief Overrided implementation of the virtual function "Update" in the
* base class "Predictor".
* @return Status::OK() if success; error otherwise.
*/
apollo::common::Status Update() override;
private:
bool PredictByImu(double old_timestamp_sec, const Pose &old_pose,
double new_timestamp_sec, Pose *new_pose);
private:
std::function<apollo::common::Status(double, const Pose &)> publish_loc_func_;
FRIEND_TEST(PredictorOutputTest, PredictByImu1);
FRIEND_TEST(PredictorOutputTest, PredictByImu2);
};
} // namespace localization
} // namespace apollo
#endif // MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_OUTPUT_H_
modules/localization/lmd/predictor/output/predictor_output_test.cc 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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/localization/lmd/predictor/output/predictor_output.h"
#include "gtest/gtest.h"
namespace apollo {
namespace localization {
using apollo::common::Status;
class PredictorOutputTest : public ::testing::Test {
public:
PredictorOutputTest()
: predictor_(10.0, [&](double, const Pose&) { return Status::OK(); }) {}
protected:
PredictorOutput predictor_;
};
TEST_F(PredictorOutputTest, PredictByImu1) {
auto& imu = predictor_.dep_predicteds_[kPredictorImuName];
imu.clear();
Pose imu_pose;
imu_pose.mutable_linear_acceleration()->set_x(1.0);
imu_pose.mutable_linear_acceleration()->set_y(1.0);
imu_pose.mutable_linear_acceleration()->set_z(0.0);
imu_pose.mutable_angular_velocity()->set_x(0.0);
imu_pose.mutable_angular_velocity()->set_y(0.0);
imu_pose.mutable_angular_velocity()->set_z(0.0);
imu.Push(0.0, imu_pose);
imu_pose.mutable_linear_acceleration()->set_x(0.5);
imu_pose.mutable_linear_acceleration()->set_y(0.5);
imu_pose.mutable_linear_acceleration()->set_z(0.0);
imu.Push(0.5, imu_pose);
imu.Push(3.0, imu_pose);
Pose old_pose;
old_pose.mutable_position()->set_x(0.0);
old_pose.mutable_position()->set_y(0.0);
old_pose.mutable_position()->set_z(0.0);
old_pose.mutable_orientation()->set_qw(1.0);
old_pose.mutable_orientation()->set_qx(0.0);
old_pose.mutable_orientation()->set_qy(0.0);
old_pose.mutable_orientation()->set_qz(0.0);
old_pose.mutable_linear_velocity()->set_x(0.0);
old_pose.mutable_linear_velocity()->set_y(0.0);
old_pose.mutable_linear_velocity()->set_z(0.0);
Pose new_pose;
EXPECT_TRUE(predictor_.PredictByImu(0.0, old_pose, 1.0, &new_pose));
EXPECT_NEAR(0.354, new_pose.position().x(), 1e-3);
EXPECT_NEAR(0.354, new_pose.position().y(), 1e-3);
EXPECT_NEAR(0.0, new_pose.position().z(), 1e-3);
EXPECT_NEAR(0.625, new_pose.linear_velocity().x(), 1e-3);
EXPECT_NEAR(0.625, new_pose.linear_velocity().y(), 1e-3);
EXPECT_NEAR(0.0, new_pose.linear_velocity().z(), 1e-3);
}
TEST_F(PredictorOutputTest, PredictByImu2) {
auto& imu = predictor_.dep_predicteds_[kPredictorImuName];
imu.clear();
Pose imu_pose;
imu_pose.mutable_linear_acceleration()->set_x(1.0);
imu_pose.mutable_linear_acceleration()->set_y(1.0);
imu_pose.mutable_linear_acceleration()->set_z(0.0);
imu_pose.mutable_angular_velocity()->set_x(0.0);
imu_pose.mutable_angular_velocity()->set_y(0.0);
imu_pose.mutable_angular_velocity()->set_z(0.0);
imu.Push(0.0, imu_pose);
imu_pose.mutable_linear_acceleration()->set_x(1.0);
imu_pose.mutable_linear_acceleration()->set_y(1.0);
imu_pose.mutable_linear_acceleration()->set_z(0.0);
imu.Push(1.0, imu_pose);
Pose old_pose;
old_pose.mutable_position()->set_x(0.0);
old_pose.mutable_position()->set_y(0.0);
old_pose.mutable_position()->set_z(0.0);
old_pose.mutable_orientation()->set_qw(1.0);
old_pose.mutable_orientation()->set_qx(0.0);
old_pose.mutable_orientation()->set_qy(0.0);
old_pose.mutable_orientation()->set_qz(0.0);
old_pose.mutable_linear_velocity()->set_x(0.0);
old_pose.mutable_linear_velocity()->set_y(0.0);
old_pose.mutable_linear_velocity()->set_z(0.0);
Pose new_pose;
EXPECT_TRUE(predictor_.PredictByImu(0.0, old_pose, 1.0, &new_pose));
EXPECT_NEAR(0.5, new_pose.position().x(), 1e-3);
EXPECT_NEAR(0.5, new_pose.position().y(), 1e-3);
EXPECT_NEAR(0.0, new_pose.position().z(), 1e-3);
EXPECT_NEAR(1.0, new_pose.linear_velocity().x(), 1e-3);
EXPECT_NEAR(1.0, new_pose.linear_velocity().y(), 1e-3);
EXPECT_NEAR(0.0, new_pose.linear_velocity().z(), 1e-3);
}
} // namespace localization
} // namespace apollo
modules/localization/lmd/predictor/output/predictor_print_error.cc 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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/localization/lmd/predictor/output/predictor_print_error.h"
#include <algorithm>
#include <iomanip>
#include "modules/common/math/math_utils.h"
#include "modules/common/math/quaternion.h"
namespace apollo {
namespace localization {
using apollo::common::Status;
using apollo::common::math::QuaternionToHeading;
using apollo::common::math::RotateAxis;
namespace {
constexpr double kPrintErrorInterval = 0.01;
} // namespace
PredictorPrintError::PredictorPrintError(double memory_cycle_sec)
: Predictor(memory_cycle_sec) {
name_ = kPredictorPrintErrorName;
dep_predicteds_.emplace(kPredictorGpsName, PoseList(memory_cycle_sec));
dep_predicteds_.emplace(kPredictorOutputName, PoseList(memory_cycle_sec));
on_adapter_thread_ = true;
}
PredictorPrintError::~PredictorPrintError() {}
bool PredictorPrintError::Updateable() const {
const auto& gps = dep_predicteds_.find(kPredictorGpsName)->second;
const auto& output = dep_predicteds_.find(kPredictorOutputName)->second;
return !gps.empty() && !output.empty() &&
(!has_latest_timestamp_sec_ ||
latest_timestamp_sec_ + kPrintErrorInterval < DepsTimestamp());
}
Status PredictorPrintError::Update() {
auto deps_timestamp_sec = DepsTimestamp();
if (!has_latest_timestamp_sec_) {
has_latest_timestamp_sec_ = true;
latest_timestamp_sec_ = deps_timestamp_sec;
return Status::OK();
}
for (auto timestamp_sec = latest_timestamp_sec_;
timestamp_sec + kPrintErrorInterval < deps_timestamp_sec;
timestamp_sec += kPrintErrorInterval) {
const auto& output = dep_predicteds_[kPredictorOutputName];
Pose pose;
output.FindMatchingPose(timestamp_sec, &pose);
PrintPoseError(timestamp_sec, pose);
}
latest_timestamp_sec_ = deps_timestamp_sec;
return Status::OK();
}
double PredictorPrintError::DepsTimestamp() const {
auto g_it = dep_predicteds_.find(kPredictorGpsName)->second.Latest();
auto o_it = dep_predicteds_.find(kPredictorOutputName)->second.Latest();
return std::min(o_it->first, g_it->first);
}
void PredictorPrintError::PrintPoseError(double timestamp_sec,
const Pose& pose) {
const auto& gps = dep_predicteds_[kPredictorGpsName];
Pose gps_pose;
if (!gps.FindMatchingPose(timestamp_sec, &gps_pose)) {
return;
}
if (!pose.has_position() || !gps_pose.has_position() ||
!pose.has_orientation() || !gps_pose.has_orientation() ||
!pose.has_linear_velocity() || !gps_pose.has_linear_velocity()) {
AERROR << "Poses has not some feilds";
return;
}
auto heading =
QuaternionToHeading(pose.orientation().qw(), pose.orientation().qx(),
pose.orientation().qy(), pose.orientation().qz());
auto gps_heading = QuaternionToHeading(
gps_pose.orientation().qw(), gps_pose.orientation().qx(),
gps_pose.orientation().qy(), gps_pose.orientation().qz());
double flu_vx, flu_vy;
RotateAxis(gps_heading, gps_pose.linear_velocity().x(),
gps_pose.linear_velocity().y(), &flu_vx, &flu_vy);
// TODO(all): estimate acceleration
double flu_ax = 0.0, flu_ay = 0.0;
Pose gps_pose0;
constexpr double kTimeDiff = 0.01;
if (gps.FindMatchingPose(timestamp_sec - kTimeDiff, &gps_pose0) &&
gps_pose0.has_orientation() && gps_pose0.has_linear_velocity()) {
auto gps_heading0 = QuaternionToHeading(
gps_pose0.orientation().qw(), gps_pose0.orientation().qx(),
gps_pose0.orientation().qy(), gps_pose0.orientation().qz());
double flu_vx0, flu_vy0;
RotateAxis(gps_heading0, gps_pose0.linear_velocity().x(),
gps_pose0.linear_velocity().y(), &flu_vx0, &flu_vy0);
flu_ax = (flu_vx - flu_vx0) / kTimeDiff;
flu_ay = (flu_vy - flu_vy0) / kTimeDiff;
}
double flu_dx, flu_dy;
RotateAxis(gps_heading, pose.position().x() - gps_pose.position().x(),
pose.position().y() - gps_pose.position().y(), &flu_dx, &flu_dy);
double flu_dvx, flu_dvy;
RotateAxis(gps_heading, pose.linear_velocity().x(),
pose.linear_velocity().y(), &flu_dvx, &flu_dvy);
flu_dvx -= flu_vx;
flu_dvy -= flu_vy;
double flu_dax, flu_day;
RotateAxis(gps_heading, pose.linear_acceleration().x(),
pose.linear_acceleration().y(), &flu_dax, &flu_day);
flu_dax -= flu_ax;
flu_day -= flu_ay;
ADEBUG << std::setprecision(15) << "Timestamp[" << timestamp_sec << "]";
ADEBUG << std::setprecision(15) << "True heading[" << gps_heading
<< "], heading error[" << heading - gps_heading << "]";
ADEBUG << std::setprecision(15) << "True position, x["
<< gps_pose.position().x() << "], y[" << gps_pose.position().y()
<< "], z[" << gps_pose.position().z() << "], position error, station["
<< flu_dx << "], lateral[" << flu_dy << "]";
ADEBUG << std::setprecision(15) << "True velocity, station[" << flu_vx
<< "], lateral[" << flu_vy << "], velocity error, station[" << flu_dvx
<< "], lateral[" << flu_dvy << "]";
ADEBUG << std::setprecision(15) << "True acceleration, station[" << flu_ax
<< "], lateral[" << flu_ay << "], acceleration error, station["
<< flu_dax << "], lateral[" << flu_day << "]";
}
} // namespace localization
} // namespace apollo
modules/localization/lmd/predictor/output/predictor_print_error.h 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file predictor_print_error.h
* @brief The class of PredictorPrintError.
*/
#ifndef MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_PRINT_ERROR_H_
#define MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_PRINT_ERROR_H_
#include "modules/localization/lmd/predictor/predictor.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace apollo {
namespace localization {
/**
* @class PredictorPrintError
*
* @brief Implementation of predictor.
*/
class PredictorPrintError : public Predictor {
public:
explicit PredictorPrintError(double memory_cycle_sec);
virtual ~PredictorPrintError();
/**
* @brief Overrided implementation of the virtual function "Updateable" in the
* base class "Predictor".
* @return True if yes; no otherwise.
*/
bool Updateable() const override;
/**
* @brief Overrided implementation of the virtual function "Update" in the
* base class "Predictor".
* @return Status::OK() if success; error otherwise.
*/
apollo::common::Status Update() override;
private:
double DepsTimestamp() const;
void PrintPoseError(double timestamp_sec, const Pose &pose);
private:
bool has_latest_timestamp_sec_ = false;
double latest_timestamp_sec_;
};
} // namespace localization
} // namespace apollo
#endif // MODULES_LOCALIZATION_LMD_PREDICTOR_OUTPUT_PREDICTOR_PRINT_ERROR_H_
modules/localization/lmd/predictor/predictor.h 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file predictor.h
* @brief The class of Predictor.
*/
#ifndef MODULES_LOCALIZATION_LMD_COMMON_PREDICTOR_PREDICTOR_H_
#define MODULES_LOCALIZATION_LMD_COMMON_PREDICTOR_PREDICTOR_H_
#include <map>
#include <string>
#include <utility>
#include "modules/common/log.h"
#include "modules/common/status/status.h"
#include "modules/localization/lmd/common/pose_list.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace apollo {
namespace localization {
constexpr char kPredictorPrintErrorName[] = "print_error";
constexpr char kPredictorOutputName[] = "output";
constexpr char kPredictorImuName[] = "imu";
constexpr char kPredictorGpsName[] = "gps";
/**
* @class Predictor
*
* @brief Interface for implementing predictor.
*/
class Predictor {
public:
explicit Predictor(double memory_cycle_sec_)
: predicted_(memory_cycle_sec_) {}
virtual ~Predictor() {}
/**
* @brief Get name of the predictor.
* @return A name.
*/
const std::string& Name() const { return name_; }
/**
* @brief Get names of the deps.
* @return Predicteds of deps.
*/
const std::map<std::string, PoseList>& DepPredicteds() const {
return dep_predicteds_;
}
/**
* @brief Update predicted list of deps.
*/
void UpdateDepPredicted(const std::string& name, const PoseList& predicted) {
UpdateDepPredicted(name, PoseList(predicted));
}
void UpdateDepPredicted(const std::string& name, PoseList&& predicted) {
auto it = dep_predicteds_.find(name);
CHECK(it != dep_predicteds_.end());
it->second = std::move(predicted);
}
/**
* @brief Get predicted list.
* @return Predicted list.
*/
const PoseList& Predicted() const { return predicted_; }
/**
* @brief Is predicted running on adapter's thread.
* @return True if yes; no otherwise.
*/
bool UpdatingOnAdapterThread() const { return on_adapter_thread_; }
/**
* @brief Is predicted list updateable.
* @return True if yes; no otherwise.
*/
virtual bool Updateable() const = 0;
/**
* @brief Predict a new pose and add it to predicted list.
* @return Status::OK() if success; error otherwise.
*/
virtual apollo::common::Status Update() = 0;
protected:
std::string name_;
std::map<std::string, PoseList> dep_predicteds_;
PoseList predicted_;
bool on_adapter_thread_ = false;
};
} // namespace localization
} // namespace apollo
#endif // MODULES_LOCALIZATION_LMD_COMMON_PREDICTOR_PREDICTOR_H_
modules/localization/lmd/predictor/raw/BUILD 0 → 100644
浏览文件 @ 7461c419
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "localization_lmd_predictor_raw_base",
deps = [
"//modules/localization/lmd/predictor:localization_lmd_predictor",
],
)
cc_library(
name = "localization_lmd_predictor_raw_imu",
srcs = [
"predictor_imu.cc",
],
hdrs = [
"predictor_imu.h",
],
deps = [
":localization_lmd_predictor_raw_base",
"//modules/common:log",
"//modules/localization/proto:imu_proto",
],
)
cc_library(
name = "localization_lmd_predictor_raw_gps",
srcs = [
"predictor_gps.cc",
],
hdrs = [
"predictor_gps.h",
],
deps = [
":localization_lmd_predictor_raw_base",
"//modules/common:log",
"//modules/localization/proto:gps_proto",
],
)
cpplint()
modules/localization/lmd/predictor/raw/predictor_gps.cc 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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 <iomanip>
#include "modules/localization/lmd/predictor/raw/predictor_gps.h"
#include "modules/common/log.h"
namespace apollo {
namespace localization {
using apollo::common::Status;
PredictorGps::PredictorGps(double memory_cycle_sec)
: Predictor(memory_cycle_sec) {
name_ = kPredictorGpsName;
on_adapter_thread_ = true;
}
PredictorGps::~PredictorGps() {}
bool PredictorGps::UpdateGps(const Gps& gps) {
if (!gps.has_header() || !gps.header().has_timestamp_sec() ||
!gps.has_localization()) {
AERROR << "Message has not some feilds";
return false;
}
auto timestamp_sec = gps.header().timestamp_sec();
if (!predicted_.Push(timestamp_sec, gps.localization())) {
AWARN << std::setprecision(15)
<< "Failed push pose to list, with timestamp[" << timestamp_sec
<< "]";
return false;
}
return true;
}
bool PredictorGps::Updateable() const {
return !predicted_.empty() && predicted_.Newer(latest_timestamp_sec_);
}
Status PredictorGps::Update() {
if (!predicted_.empty()) {
auto latest = predicted_.Latest();
latest_timestamp_sec_ = latest->first;
}
return Status::OK();
}
} // namespace localization
} // namespace apollo
modules/localization/lmd/predictor/raw/predictor_gps.h 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file predictor.h
* @brief The class of PredictorGps.
*/
#ifndef MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_GPS_H_
#define MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_GPS_H_
#include "modules/localization/proto/gps.pb.h"
#include "modules/localization/lmd/predictor/predictor.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace apollo {
namespace localization {
/**
* @class PredictorImu
*
* @brief Implementation of predictor.
*/
class PredictorGps : public Predictor {
public:
explicit PredictorGps(double memory_cycle_sec);
virtual ~PredictorGps();
/**
* @brief Update poses from gps.
* @param gps The message from gps.
* @return True if success; false if not needed.
*/
bool UpdateGps(const Gps &gps);
/**
* @brief Overrided implementation of the virtual function "Updateable" in the
* base class "Predictor".
* @return True if yes; no otherwise.
*/
bool Updateable() const override;
/**
* @brief Overrided implementation of the virtual function "Update" in the
* base class "Predictor".
* @return Status::OK() if success; error otherwise.
*/
apollo::common::Status Update() override;
private:
double latest_timestamp_sec_;
};
} // namespace localization
} // namespace apollo
#endif // MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_GPS_H_
modules/localization/lmd/predictor/raw/predictor_imu.cc 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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 <iomanip>
#include "modules/localization/lmd/predictor/raw/predictor_imu.h"
#include "modules/common/log.h"
namespace apollo {
namespace localization {
using apollo::common::Status;
PredictorImu::PredictorImu(double memory_cycle_sec)
: Predictor(memory_cycle_sec), raw_imu_(memory_cycle_sec) {
name_ = kPredictorImuName;
on_adapter_thread_ = true;
}
PredictorImu::~PredictorImu() {}
bool PredictorImu::UpdateImu(const CorrectedImu& imu) {
if (!imu.has_header() || !imu.header().has_timestamp_sec() ||
!imu.has_imu()) {
AERROR << "Message has not some feilds";
return false;
}
auto timestamp_sec = imu.header().timestamp_sec();
if (!raw_imu_.Push(timestamp_sec, imu.imu())) {
AWARN << std::setprecision(15)
<< "Failed push pose to list, with timestamp[" << timestamp_sec
<< "]";
return false;
}
return WindowFilter(timestamp_sec);
}
bool PredictorImu::Updateable() const {
return !predicted_.empty() && predicted_.Newer(latest_timestamp_sec_);
}
Status PredictorImu::Update() {
if (!predicted_.empty()) {
auto latest = predicted_.Latest();
latest_timestamp_sec_ = latest->first;
}
return Status::OK();
}
bool PredictorImu::WindowFilter(double timestamp_sec) {
Pose pose;
pose.mutable_linear_acceleration()->set_x(0.0);
pose.mutable_linear_acceleration()->set_y(0.0);
pose.mutable_linear_acceleration()->set_z(0.0);
pose.mutable_angular_velocity()->set_x(0.0);
pose.mutable_angular_velocity()->set_y(0.0);
pose.mutable_angular_velocity()->set_z(0.0);
constexpr double kWindowSize = 0.20;
constexpr double kSamplingInterval = 0.01;
std::size_t count = 0;
for (auto t = timestamp_sec; t >= timestamp_sec - kWindowSize;
t -= kSamplingInterval) {
Pose sample;
raw_imu_.FindNearestPose(t, &sample);
pose.mutable_linear_acceleration()->set_x(pose.linear_acceleration().x() +
sample.linear_acceleration().x());
pose.mutable_linear_acceleration()->set_y(pose.linear_acceleration().y() +
sample.linear_acceleration().y());
pose.mutable_linear_acceleration()->set_z(pose.linear_acceleration().z() +
sample.linear_acceleration().z());
pose.mutable_angular_velocity()->set_x(pose.angular_velocity().x() +
sample.angular_velocity().x());
pose.mutable_angular_velocity()->set_y(pose.angular_velocity().y() +
sample.angular_velocity().y());
pose.mutable_angular_velocity()->set_z(pose.angular_velocity().z() +
sample.angular_velocity().z());
count++;
}
pose.mutable_linear_acceleration()->set_x(pose.linear_acceleration().x() /
count);
pose.mutable_linear_acceleration()->set_y(pose.linear_acceleration().y() /
count);
pose.mutable_linear_acceleration()->set_z(pose.linear_acceleration().z() /
count);
pose.mutable_angular_velocity()->set_x(pose.angular_velocity().x() / count);
pose.mutable_angular_velocity()->set_y(pose.angular_velocity().y() / count);
pose.mutable_angular_velocity()->set_z(pose.angular_velocity().z() / count);
if (!predicted_.Push(timestamp_sec, pose)) {
AWARN << std::setprecision(15)
<< "Failed push pose to list, with timestamp[" << timestamp_sec
<< "]";
return false;
}
return true;
}
} // namespace localization
} // namespace apollo
modules/localization/lmd/predictor/raw/predictor_imu.h 0 → 100644
浏览文件 @ 7461c419
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file predictor.h
* @brief The class of PredictorImu.
*/
#ifndef MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_IMU_H_
#define MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_IMU_H_
#include "modules/localization/proto/imu.pb.h"
#include "modules/localization/lmd/predictor/predictor.h"
/**
* @namespace apollo::localization
* @brief apollo::localization
*/
namespace apollo {
namespace localization {
/**
* @class PredictorImu
*
* @brief Implementation of predictor.
*/
class PredictorImu : public Predictor {
public:
explicit PredictorImu(double memory_cycle_sec);
virtual ~PredictorImu();
/**
* @brief Update poses from imu.
* @param imu The message from imu.
* @return True if success; false if not needed.
*/
bool UpdateImu(const CorrectedImu& imu);
/**
* @brief Overrided implementation of the virtual function "Updateable" in the
* base class "Predictor".
* @return True if yes; no otherwise.
*/
bool Updateable() const override;
/**
* @brief Overrided implementation of the virtual function "Update" in the
* base class "Predictor".
* @return Status::OK() if success; error otherwise.
*/
apollo::common::Status Update() override;
private:
bool WindowFilter(double timestamp_sec);
private:
double latest_timestamp_sec_;
PoseList raw_imu_;
};
} // namespace localization
} // namespace apollo
#endif // MODULES_LOCALIZATION_LMD_PREDICTOR_RAW_PREDICTOR_IMU_H_
modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.cc
浏览文件 @ 7461c419
......@@ -579,7 +579,10 @@ bool CCLanePostProcessor::ProcessWithoutCC(
Eigen::Matrix<double, 3, 1> xy_p = trans_mat_ * img_point;
Eigen::Matrix<double, 2, 1> xy_point;
Eigen::Matrix<double, 2, 1> uv_point;
if (std::abs(xy_p(2)) < 1e-6) continue;
if (std::abs(xy_p(2)) < 1e-6) {
++x;
continue;
}
xy_point << xy_p(0) / xy_p(2), xy_p(1) / xy_p(2);
if (xy_point(0) < 0 || xy_point(0) > 300 || abs(xy_point(1)) > 30) {
++x;
......
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或