Commit bede62de authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

WIP

parent bbe4cf23
......@@ -448,7 +448,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, t0, x_origin);
FrameBasePtr KF1 = problem->emplaceKeyFrame(KEY, t0, x_origin);
// 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();
......@@ -762,7 +762,8 @@ int main (int argc, char **argv) {
VectorComposite kf_state;
CaptureBasePtr cap_ikin;
VectorComposite bp_bias_est;
for (auto kf: problem->getTrajectory()->getFrameList()){
for (auto& elt: problem->getTrajectory()->getFrameMap()){
auto kf = elt.second;
if (kf->isKey()){
kf_state = kf->getState();
cap_ikin = kf->getCaptureOf(sen_ikin);
......@@ -853,7 +854,7 @@ int main (int argc, char **argv) {
// // Print factor energy
// for (auto kf: problem->getTrajectory()->getFrameList()){
// for (auto kf: problem->getTrajectory()->getFrameMap()){
// for (auto cap: kf->getCaptureList()){
// for (auto feat: cap->getFeatureList()){
// for (auto fac: feat->getFactorList()){
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment