Skip to content
Snippets Groups Projects

WIP: Resolve "Adapt to std::set and std::map instead of std::list in wolf nodes"

Files
5
@@ -432,7 +432,7 @@ int main (int argc, char **argv) {
P_origin.block<3,3>(0,0) = pow(std_prior_p, 2) * Matrix3d::Identity();
P_origin.block<3,3>(3,3) = pow(std_prior_o, 2) * Matrix3d::Identity();
P_origin.block<3,3>(6,6) = pow(std_prior_v, 2) * Matrix3d::Identity();
FrameBasePtr KF1 = problem->emplaceFrame(KEY, x_origin, t0);
FrameBasePtr KF1 = problem->emplaceKeyFrame( x_origin, t0);
// Prior pose factor
CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
pose_prior_capture->emplaceFeatureAndFactor();
Loading