okvis

将文件okvis_app_synchronous.cpp中代码修改为:

 
class PoseViewer
{
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  constexpr static const double imageSize = 500.0;
  PoseViewer()
  {
    cv::namedWindow("OKVIS Top View");
    _image.create(imageSize, imageSize, CV_8UC3);
    drawing_ = false;
    showing_ = false;
  }
  // this we can register as a callback
  void publishFullStateAsCallback(
      const okvis::Time & t, const okvis::kinematics::Transformation & T_WS,
      const Eigen::Matrix<double, 9, 1> & speedAndBiases,
      const Eigen::Matrix<double, 3, 1> & /*omega_S*/)
  {

    Eigen::Vector3d p_WS_W = T_WS.r();
    Eigen::Quaterniond q_WS = T_WS.q();
    std::ofstream foutC("/home/slam/okvis/trajecty.txt", std::ios::app);
    foutC.setf(std::ios::fixed, std::ios::floatfield);
    foutC.precision(5);
    foutC << t.sec << "." << t.nsec << " "
             << p_WS_W(0) << " " << p_WS_W(1) << " " <<  p_WS_W(2) << " "
             << q_WS.x() << " " << q_WS.y() << " " << q_WS.z() << " " << q_WS.w()
             << std::endl;
    foutC.close();

        Eigen::Vector3d p_WS_W = T_WS.r();
    Eigen::Quaterniond q_WS = T_WS.q();
    std::ofstream foutC("/home/slam/okvis/trajecty.txt", std::ios::app);
    foutC.setf(std::ios::fixed, std::ios::floatfield);
    foutC.precision(5);
    foutC << t.sec << "." << t.nsec << " "
             << p_WS_W(0) << " " << p_WS_W(1) << " " <<  p_WS_W(2) << " "
             << q_WS.x() << " " << q_WS.y() << " " << q_WS.z() << " " << q_WS.w()
             << std::endl;
    foutC.close();

 

 

找到代码:

std::atomic_bool showing_;

在后面添加:

std::ofstream ofs_traj;