Refactor a bit for clarity and organization

src/publisher_vision.cpp

@@ -322,14 +322,17 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
 
 Eigen::Isometry3d PublisherVisionDebug::getTcw(const CaptureBasePtr _capture) const
 {
-    auto state = _capture->getProblem()->getState(_capture->getTimeStamp());
-    auto pos = state['P'];
-    auto orientation = state['O'].data();
-
-    Eigen::Isometry3d T_w_r = Translation<double, 3>(pos) * Quaterniond(orientation);
-    Eigen::Isometry3d T_r_c = Translation<double, 3>(_capture->getSensorP()->getState())
+    // get robot position and orientation at capture's timestamp
+    const auto& state   = _capture->getProblem()->getState(_capture->getTimeStamp());
+    const auto& pos     = state.at('P');
+    const auto& ori     = state.at('O');
+
+    // compute world-to-camera transform
+    Eigen::Isometry3d T_w_r = Translation3d(pos) * Quaterniond(ori.data());
+    Eigen::Isometry3d T_r_c = Translation3d(_capture->getSensorP()->getState())
             * Quaterniond(_capture->getSensorO()->getState().data());
 
+    // invert transform to camera-to-world and return
     return (T_w_r * T_r_c).inverse();
 }
 
@@ -343,7 +346,9 @@ Eigen::Matrix<double, 3, 4> PublisherVisionDebug::getProjectionMatrix(const Capt
     auto k(_capture->getSensor()->getIntrinsic()->getState());
     Eigen::Matrix<double, 3, 4> P;
 
-    P << k(2), 0, k(0), 0, 0, k(3), k(1), 0, 0, 0, 1, 0;
+    P <<    k(2), 0   , k(0), 0, //
+            0   , k(3), k(1), 0, //
+            0   , 0   , 1   , 0; //
 
     return P;
 }