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(); }