1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
#if (!USE_GPS_ALIGNMENT_LOCAL_FREAME)
if (first_submap) {
first_submap = false;
// Fix the first submap of the first trajectory except for allowing
// gravity alignment.
// 重力是否需要对齐与此参数是否内优化的关系在哪里体现？ConstantYawQuaternionPlus吗？
C_submaps.Insert(
submap_id_data.id,
CeresPose(submap_id_data.data.global_pose,
translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization<
ConstantYawQuaternionPlus, 4, 2>>(),
&problem));
// 当利用ENU坐标系时，是否需要调整首节点的位置呢？
// （需要：不仅要对齐x，y轴，也要对齐原点，因为采样时刻的不同步，首节点不一定时首次GPS的位置点）
problem.SetParameterBlockConstant(
C_submaps.at(submap_id_data.id).translation());
}
else
#endif // USE_GPS_ALIGNMENT_LOCAL_FREAME
{
C_submaps.Insert(
submap_id_data.id,
CeresPose(submap_id_data.data.global_pose,
translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(),
&problem));
}

1
2
3
4
5
6
// 约束方程： I = fixed_frame_pose_in_map.inverse()*node_pose_in_map;
//
constraint_pose = C_enu_frames.inverse()*C_nodes.pose;
//
或者：C_enu_frames = I*C_nodes.pose;

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
#if (USE_GPS_ALIGNMENT_LOCAL_FREAME)
std::map<NodeId, CeresPose> C_enu_frames;
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) {
node_it = trajectory_end;
continue;
}
double last_x(0), last_y(0);
int use_fix_num(0);
double s_sum(0);
bool first_points = true;
double yawl_from_fix(0);
std::unique_ptr<transform::Rigid3d> fixed_frame_pose_last;
for (; node_it != trajectory_end; ++node_it) {
const NodeId node_id = node_it->id;
const NodeSpec3D& node_data = node_it->data;
if (first_points)
{
first_points = false;
last_x = node_data.global_pose.translation().x();
last_y = node_data.global_pose.translation().y();
}
constexpr double ds_min_gps = 0.1;
double ds = hypot(node_data.global_pose.translation().x() - last_x,
node_data.global_pose.translation().y() - last_y);
if (ds < ds_min_gps)
{
// printf("ds:%1.2f, ", ds);
continue;
}
// 基于fixed_frame_pose_data_做插补运算
// const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
//
Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);
int8_t gps_status = GPS_STATUS_NO_FIX;
const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
Interpolate_with_gps_status(fixed_frame_pose_data_, trajectory_id, node_data.time, gps_status);
if (fixed_frame_pose == nullptr) {
// printf("fixed_frame_pose is nullptr, ");
continue;
}
double k_weight_by_gps_status = 1.0;
switch (gps_status)
{
case GPS_STATUS_FIX:
k_weight_by_gps_status = 0.2;
break;
case GPS_STATUS_SBAS_FIX:
k_weight_by_gps_status = 0.5;
break;
case GPS_STATUS_GBAS_FIX:
k_weight_by_gps_status = 1.0;
break;
default:
k_weight_by_gps_status = 0.0;
break;
}
last_x = node_data.global_pose.translation().x();
last_y = node_data.global_pose.translation().y();
if (fixed_frame_pose_last != nullptr)
{
yawl_from_fix = atan2(fixed_frame_pose->translation().y() - fixed_frame_pose_last->translation().y(),
fixed_frame_pose->translation().x() - fixed_frame_pose_last->translation().x());
// printf("yawl_from_fix = %1.4f degree, ",yawl_from_fix*180.0/M_PI);
fixed_frame_pose_last = common::make_unique<transform::Rigid3d>(*fixed_frame_pose);
}
else
{
// printf("fixed_frame_pose_last is nullptr, ");
fixed_frame_pose_last = common::make_unique<transform::Rigid3d>(*fixed_frame_pose);
continue;
}
// 一次有效的约束计算，更新x，y记录值；
use_fix_num ++;
s_sum += ds;
// ----------------------------------------------------------------------
C_enu_frames.emplace(
std::piecewise_construct, std::forward_as_tuple(node_id),
std::forward_as_tuple(
transform::Rigid3d(),
nullptr,
// common::make_unique<ceres::AutoDiffLocalParameterization<
//
YawOnlyQuaternionPlus, 4, 1>>(),
// common::make_unique<ceres::AutoDiffLocalParameterization<
//
ConstantYawQuaternionPlus, 4, 2>>(),
common::make_unique<ceres::QuaternionParameterization>(),
&problem));
problem.SetParameterBlockConstant(
C_enu_frames.at(node_id).rotation());
problem.SetParameterBlockConstant(
C_enu_frames.at(node_id).translation());
const Constraint::Pose constraint_pose{
transform::Rigid3d(
fixed_frame_pose->translation(),
Eigen::AngleAxisd(
yawl_from_fix,
Eigen::Vector3d::UnitZ())),
options_.fixed_frame_pose_translation_weight()*k_weight_by_gps_status,
options_.fixed_frame_pose_rotation_weight()*k_weight_by_gps_status};
// 约束方程： I = fixed_frame_pose_in_map.inverse()*node_pose_in_map;
//
constraint_pose = C_enu_frames.inverse()*C_nodes.pose;
//
或者：C_enu_frames = I*C_nodes.pose;
problem.AddResidualBlock(
SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose),
nullptr /* loss function */,
C_enu_frames.at(node_id).rotation(),
C_enu_frames.at(node_id).translation(),
C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation());
}
printf("nuse_fix_num:%d, s_sum:%1.3fnn", use_fix_num, s_sum);
}
#endif

1
2
3
4
5
6
7
8
9
10

has_rec_origin_ = false;
if (map_builder_bridge_.sensor_bridge(trajectory_id)->has_ecef_to_local_frame())
{
has_rec_origin_ = true;
rec_origin_fixed_to_local_frame_ =
map_builder_bridge_.sensor_bridge(trajectory_id)->get_ecef_to_local_frame();
rec_traj_pose_ = map_builder_bridge_.GetSubmapList().submap.begin()->pose;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

if (has_rec_origin_)
{
printf("nnfilename:%sn n",
filename.c_str());
std::string mapmetadatafile = filename + ".yaml";
printf("Writing map occupancy data to %s", mapmetadatafile.c_str());
FILE* yaml = fopen(mapmetadatafile.c_str(), "w");
fprintf(yaml, "ecef_frame_to_enu_frame:n");
fprintf(yaml, "
translation:n
{x: %1.15f,n
y: %1.15f,n
z: %1.15f}n",
rec_origin_fixed_to_local_frame_.translation().x(),
rec_origin_fixed_to_local_frame_.translation().y(),
rec_origin_fixed_to_local_frame_.translation().z());
fprintf(yaml, "
rotation:n
{x: %1.15f,n
y: %1.15f,n
z: %1.15f,n
w: %1.15f}n",
rec_origin_fixed_to_local_frame_.rotation().x(),
rec_origin_fixed_to_local_frame_.rotation().y(),
rec_origin_fixed_to_local_frame_.rotation().z(),
rec_origin_fixed_to_local_frame_.rotation().w());
fprintf(yaml, "map_traj0:n");
fprintf(yaml, "
translation:n
{x: %1.15f,n
y: %1.15f,n
z: %1.15f}n",
rec_traj_pose_.position.x,
rec_traj_pose_.position.y,
rec_traj_pose_.position.z);
fprintf(yaml, "
rotation:n
{x: %1.15f,n
y: %1.15f,n
z: %1.15f,n
w: %1.15f}n",
rec_traj_pose_.orientation.x,
rec_traj_pose_.orientation.y,
rec_traj_pose_.orientation.z,
rec_traj_pose_.orientation.w);
fclose(yaml);
printf("nDonennn");
}
else
{
printf("nnNO UTM_to_LOCAL when stop traj!!!nnn");
}

1
2
3
4
5
if (base_on_map)
{
node.set_traj_ecef_to_local_frame(traj_id, ecef_to_enu);
}

1
2
3
4
5
6
7
有地图的yaml文件，且有gps信号；
if ((!FLAGS_load_state_filename.empty())&&(is_gnss_fix_ready))
{
....
base_on_map = true;
}

1
2
3
4
1、更改node中的StartTrajectoryWithDefaultTopics()函数，返回新建的轨迹编号；
2、在node中增加函数set_traj_ecef_to_local_frame()设置坐标变换关系；
3、在sensor_bridge中增加函数set_ecef_to_local_frame()设置坐标变换关系；

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
根据传递的ros参数set_init_pose_by_gps_pose，需要使用gps进行初始定位时，则
创建订阅话题：
ros::Subscriber
fix_sub = node.node_handle()->subscribe("/fix", 1, fix_callback);
ros::Subscriber
imu_sub = node.node_handle()->subscribe("/imu", 1, imu_callback);
执行任务
car_in_enu_position = ecef_to_enu*ecef_position_;
car_in_traj0_pose = enu_to_traj0*car_in_enu_pose;
在执行完毕任务后，删除话题订阅；
fix_sub.shutdown();
fix_sub.~Subscriber();
imu_sub.shutdown();
imu_sub.~Subscriber();

1
2
3
4
5
6
struct FixedFramePoseData {
common::Time time;
common::optional<transform::Rigid3d> pose;
int8_t gps_status;
};

1
2
3
4
5
6
7
8
9

STATUS_NO_FIX = -1,
// 无固定解
STATUS_FIX = 0,
// 未增强的定位信号
STATUS_SBAS_FIX = 1,
// 基于卫星信号增强的定位信号
STATUS_GBAS_FIX = 2, // 基于地面基站增强的定位信号

1
2
3
4
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::FixedFramePoseData{
time, carto::common::optional<Rigid3d>(), msg->status.status});

1
2
3
4
5
6
7
8
9
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::FixedFramePoseData{
time, carto::common::optional<Rigid3d>(Rigid3d::Translation(
ecef_to_local_frame_.value() *
LatLongAltToEcef(msg->latitude, msg->longitude,
0.0*msg->altitude))),
msg->status.status});

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

// For fixed frame pose with status.
#define GPS_STATUS_NO_FIX
(-1)
#define GPS_STATUS_FIX
(0)
#define GPS_STATUS_SBAS_FIX
(1)
#define GPS_STATUS_GBAS_FIX
(2)
std::unique_ptr<transform::Rigid3d> Interpolate_with_gps_status(
const sensor::MapByTime<sensor::FixedFramePoseData>& map_by_time,
const int trajectory_id, const common::Time time, int8_t& gps_status) {
const auto it = map_by_time.lower_bound(trajectory_id, time);
if (it == map_by_time.EndOfTrajectory(trajectory_id) ||
!it->pose.has_value()) {
return nullptr;
}
if (it->gps_status == GPS_STATUS_NO_FIX) return nullptr;
if (it == map_by_time.BeginOfTrajectory(trajectory_id)) {
if (it->time == time) {
gps_status = it->gps_status;
return common::make_unique<transform::Rigid3d>(it->pose.value());
}
return nullptr;
}
const auto prev_it = std::prev(it);
if (prev_it->gps_status == GPS_STATUS_NO_FIX) return nullptr;
if (prev_it->pose.has_value()) {
// 增加距离判断，两点之间的距离大于设定值，则认为这段位置内不存在gps信号：
constexpr double lose_gps_distance = 1.0;
const double l_two_points
= sqrt(pow(prev_it->pose.value().translation().x() - it->pose.value().translation().x(), 2)
+ pow(prev_it->pose.value().translation().y() - it->pose.value().translation().y(), 2)
+ pow(prev_it->pose.value().translation().z() - it->pose.value().translation().z(), 2));
if (l_two_points > lose_gps_distance)
{
printf("n>>>>>WARN: gps data discard because of l_two_points = %1.2fn", l_two_points);
printf("gps1[x:%1.2f, y:%1.2f, z:%1.2f]n",
prev_it->pose.value().translation().x(),
prev_it->pose.value().translation().y(),
prev_it->pose.value().translation().z());
printf("gps2[x:%1.2f, y:%1.2f, z:%1.2f]nn",
it->pose.value().translation().x(),
it->pose.value().translation().y(),
it->pose.value().translation().z());
return nullptr;
}
gps_status = std::min(prev_it->gps_status, it->gps_status);
// --- end ---
return common::make_unique<transform::Rigid3d>(
Interpolate(transform::TimestampedTransform{prev_it->time,
prev_it->pose.value()},
transform::TimestampedTransform{it->time, it->pose.value()},
time)
.transform);
}
return nullptr;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
double k_weight_by_gps_status = 1.0;
switch (gps_status)
{
case GPS_STATUS_FIX:
k_weight_by_gps_status = 0.2;
break;
case GPS_STATUS_SBAS_FIX:
k_weight_by_gps_status = 0.5;
break;
case GPS_STATUS_GBAS_FIX:
k_weight_by_gps_status = 1.0;
break;
default:
k_weight_by_gps_status = 0.0;
break;
}

1
2
3
4
5
6
7
8
9
const Constraint::Pose constraint_pose{
transform::Rigid3d(
fixed_frame_pose->translation(),
Eigen::AngleAxisd(
yawl_from_fix,
Eigen::Vector3d::UnitZ())),
options_.fixed_frame_pose_translation_weight()*k_weight_by_gps_status,
options_.fixed_frame_pose_rotation_weight()*k_weight_by_gps_status};