本文整理汇总了C++中Pose::Delta方法的典型用法代码示例。如果您正苦于以下问题：C++ Pose::Delta方法的具体用法？C++ Pose::Delta怎么用？C++ Pose::Delta使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Pose的用法示例。
在下文中一共展示了Pose::Delta方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: EstimatePose
Status KeyframeVisualOdometry::EstimatePose(ViewIndex view_index) {
View::Ptr view = View::GetView(view_index);
CHECK_NOTNULL(view.get());
// Use PnP RANSAC to find the pose of this camera using the 2D<-->3D matches.
PnPRansacProblem pnp_problem;
pnp_problem.SetIntrinsics(intrinsics_);
std::vector<Observation::Ptr> matched_observations;
view->MatchedObservations(&matched_observations);
printf("(4) at the top, had %lu matches\n", matched_observations.size());
// Make sure the observations are only of triangulated landmarks.
std::vector<Observation::Ptr> valid_observations;
for (const auto& observation : matched_observations) {
CHECK_NOTNULL(observation.get());
if (observation->GetLandmark()->IsEstimated())
valid_observations.push_back(observation);
}
pnp_problem.SetData(valid_observations);
printf("valid observations: %lu\n", valid_observations.size());
Ransac<Observation::Ptr, PnPRansacModel> pnp_solver;
pnp_solver.SetOptions(options_.pnp_ransac_options);
pnp_solver.Run(pnp_problem);
// If RANSAC fails, set that the next frame should be a keyframe and return.
if (!pnp_problem.SolutionFound()) {
initialize_new_keyframe_ = true;
return Status::Cancelled(
"Failed to compute new camera pose with PnP RANSAC.");
}
// Get the camera pose from RANSAC.
const CameraExtrinsics& computed_extrinsics =
pnp_problem.Model().camera_.Extrinsics();
view->MutableCamera().SetExtrinsics(computed_extrinsics);
// If the computed relative translation from the last keyframe is large
// enough, it's time to initialize a new keyframe on the next iteration.
View::Ptr keyframe = View::GetView(current_keyframe_);
CHECK_NOTNULL(keyframe.get());
const Pose T1 = keyframe->Camera().Extrinsics().WorldToCamera();
const Pose T2 = computed_extrinsics.WorldToCamera();
const Pose delta = T1.Delta(T2);
if (delta.Translation().norm() > options_.min_keyframe_translation ||
delta.AxisAngle().norm() > options_.min_keyframe_rotation) {
initialize_new_keyframe_ = true;
}
return Status::Ok();
}