From 86f2e4807fd4dad2d1fc156e89be2f8cac6337fe Mon Sep 17 00:00:00 2001
From: Vladyslav Usenko <vlad.usenko@tum.de>
Date: Fri, 12 Jul 2019 14:05:07 +0200
Subject: [PATCH] small fix

---
 src/vi_estimator/nfr_mapper.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp
index c0fcb24..7a8930e 100644
--- a/src/vi_estimator/nfr_mapper.cpp
+++ b/src/vi_estimator/nfr_mapper.cpp
@@ -626,8 +626,8 @@ void NfrMapper::setup_opt() {
 
       Sophus::SE3d T_w_h = frame_poses.at(tcid_h.frame_id).getPose() *
                            calib.T_i_c[tcid_h.cam_id];
-      Sophus::SE3d T_w_o =
-          frame_poses.at(tcid_o.cam_id).getPose() * calib.T_i_c[tcid_o.cam_id];
+      Sophus::SE3d T_w_o = frame_poses.at(tcid_o.frame_id).getPose() *
+                           calib.T_i_c[tcid_o.cam_id];
 
       Eigen::Vector4d pos_3d = triangulate(
           pos_3d_h.head<3>(), pos_3d_o.head<3>(), T_w_h.inverse() * T_w_o);