12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix the size of residuals, add test for jacobians. (#895) 

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
master
Alexander Belyaev 2018-02-09 13:13:17 +01:00 committed by GitHub
parent ba113e4e05
commit b72bbb20c3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 28 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping_2d/pose_graph/landmark_cost_function.h
View File

@ -43,7 +43,7 @@ class LandmarkCostFunction {
const LandmarkObservation& observation, const NodeData& prev_node, const LandmarkObservation& observation, const NodeData& prev_node,
const NodeData& next_node) { const NodeData& next_node) {
return new ceres::AutoDiffCostFunction< return new ceres::AutoDiffCostFunction<
LandmarkCostFunction, 3 /* residuals */, LandmarkCostFunction, 6 /* residuals */,
3 /* previous node pose variables */, 3 /* next node pose variables */, 3 /* previous node pose variables */, 3 /* next node pose variables */,
4 /* landmark rotation variables */, 4 /* landmark rotation variables */,
3 /* landmark translation variables */>( 3 /* landmark translation variables */>(

20
cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc
View File

@ -49,17 +49,21 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
}, },
prev_node, next_node); prev_node, next_node);
std::array<double, 3> prev_node_pose{{2., 0., 0.}}; const std::array<double, 3> prev_node_pose{{2., 0., 0.}};
std::array<double, 3> next_node_pose{{0., 2., 0.}}; const std::array<double, 3> next_node_pose{{0., 2., 0.}};
std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}}; const std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}};
std::array<double, 3> landmark_translation{{1., 2., 1.}}; const std::array<double, 3> landmark_translation{{1., 2., 1.}};
const double* parameter_blocks[] = { const std::array<const double*, 4> parameter_blocks{
prev_node_pose.data(), next_node_pose.data(), landmark_rotation.data(), {prev_node_pose.data(), next_node_pose.data(), landmark_rotation.data(),
landmark_translation.data()}; landmark_translation.data()}};
std::array<double, 6> residuals; std::array<double, 6> residuals;
cost_function->Evaluate(parameter_blocks, residuals.data(), nullptr); std::array<std::array<double, 13>, 6> jacobians;
std::array<double*, 6> jacobians_ptrs;
for (int i = 0; i < 6; ++i) jacobians_ptrs[i] = jacobians[i].data();
cost_function->Evaluate(parameter_blocks.data(), residuals.data(),
jacobians_ptrs.data());
EXPECT_THAT(residuals, ElementsAre(DoubleEq(1.), DoubleEq(0.), DoubleEq(0.), EXPECT_THAT(residuals, ElementsAre(DoubleEq(1.), DoubleEq(0.), DoubleEq(0.),
DoubleEq(0.), DoubleEq(0.), DoubleEq(0.))); DoubleEq(0.), DoubleEq(0.), DoubleEq(0.)));
} }

25
cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc
View File

@ -47,20 +47,23 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
}, },
prev_node, next_node); prev_node, next_node);
std::array<double, 4> prev_node_rotation{{1., 0., 0., 0.}}; const std::array<double, 4> prev_node_rotation{{1., 0., 0., 0.}};
std::array<double, 3> prev_node_translation{{0., 0., 0.}}; const std::array<double, 3> prev_node_translation{{0., 0., 0.}};
std::array<double, 4> next_node_rotation{{1., 0., 0., 0.}}; const std::array<double, 4> next_node_rotation{{1., 0., 0., 0.}};
std::array<double, 3> next_node_translation{{2., 2., 2.}}; const std::array<double, 3> next_node_translation{{2., 2., 2.}};
std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}}; const std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}};
std::array<double, 3> landmark_translation{{1., 2., 2.}}; const std::array<double, 3> landmark_translation{{1., 2., 2.}};
const double* parameter_blocks[] = { const std::array<const double*, 6> parameter_blocks{
prev_node_rotation.data(), prev_node_translation.data(), {prev_node_rotation.data(), prev_node_translation.data(),
next_node_rotation.data(), next_node_translation.data(), next_node_rotation.data(), next_node_translation.data(),
landmark_rotation.data(), landmark_translation.data()}; landmark_rotation.data(), landmark_translation.data()}};
std::array<double, 6> residuals; std::array<double, 6> residuals;
cost_function->Evaluate(parameter_blocks, residuals.data(), nullptr); std::array<std::array<double, 21>, 6> jacobians;
std::array<double*, 6> jacobians_ptrs;
for (int i = 0; i < 6; ++i) jacobians_ptrs[i] = jacobians[i].data();
cost_function->Evaluate(parameter_blocks.data(), residuals.data(),
jacobians_ptrs.data());
EXPECT_THAT(residuals, ElementsAre(DoubleEq(1.), DoubleEq(0.), DoubleEq(0.), EXPECT_THAT(residuals, ElementsAre(DoubleEq(1.), DoubleEq(0.), DoubleEq(0.),
DoubleEq(0.), DoubleEq(0.), DoubleEq(0.))); DoubleEq(0.), DoubleEq(0.), DoubleEq(0.)));
} }