diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp index 55d47e2d550991f5dcaeec5084cd5fcdec7842a9..d2a04a4605ab8cb810beb7c9cc7c7ecf314aa05d 100644 --- a/src/publisher_vision.cpp +++ b/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; }