text: ///对camera.h中的各类函数进行定义实现
  
  #include "myslam/camera.h"
  
  namespace myslam {
  
      Camera::Camera() {
      }
  
      Vec3 Camera::world2camera(const Vec3 &p_w, const SE3 &T_c_w) {
          return pose_ * T_c_w * p_w;
  
      }
  
      Vec3 Camera::camera2world(const Vec3 &p_c, const SE3 &T_c_w) {
          return T_c_w.inverse() * pose_inv_ * p_c;
      }
  
      Vec2 Camera::camera2pixel(const Vec3 &p_c) {
          return Vec2(
                  fx_ * p_c(0, 0) / p_c(2, 0) + cx_,
                  fy_ * p_c(1, 0) / p_c(2, 0) + cy_
          );
      }
  
      Vec3 Camera::pixel2camera(const Vec2 &p_p, double depth) {
          return Vec3(
                  (p_p(0, 0) - cx_) * depth / fx_,
                  (p_p(1, 0) - cy_) * depth / fy_,
                  depth
          );
      }
  
      Vec2 Camera::world2pixel(const Vec3 &p_w, const SE3 &T_c_w) {
          return camera2pixel(world2camera(p_w, T_c_w));
      }
  
      Vec3 Camera::pixel2world(const Vec2 &p_p, const SE3 &T_c_w, double depth) {
          return camera2world(pixel2camera(p_p, depth), T_c_w);
      }
  
  }