code example of AIBOX DroneBrain

common.h

#ifndef SLAM_COMMON_H
#define SLAM_COMMON_H

#include <opencv2/opencv.hpp>
#include <vector>

using namespace cv;

#define FOCAL_LENGTH 374.6706070969281

inline double calcDistance(const Point3f &p1, const Point3f &p2){
  double dx=p1.x-p2.x;
  double dy=p1.y-p2.y;
  double dz=p1.z-p2.z;
  return sqrt(dx*dx+dy*dy+dz*dz);
}

struct Pose {
  Point3f p;
  Point3f o;

  Pose() {
    p = o = Point3f(0,0,0);
  }

  Pose(Point3f &_p,Point3f &_o) {
    p = _p;
    o = _o;
  }

  Pose(const Pose &that) {
    p = that.p;
    o = that.o;
  }

  Pose(const double *cam) {
    fromCamera(cam);
  }

  Pose(Mat &R,Mat &T) {
    fromCamera(R,T);
  }

  // when x --------RT--------> z
  //      x --RT1--> y --RT2--> z
  // then, 
  //   z = R*x+T
  //   y = R1*x+T1
  //   z = R2*y+T2
  // therefore,
  //   z = R2*(R1*x+T1)+T2 = R*x+T
  // to be identical equetion, we get
  //   R = R2*R1
  //   T = R2*T1+T2
  // 
  // and note that:
  //  THIS OPERATOR IS NOT EXCHANGEABLE.
  //  BE CAREFUL COMPARED TO ORDINAL ADD
  //  OPERATOR WHICH CAN BE EXCHANGE.
  Pose operator+(const Pose &that) {
    Mat R1,T1,R2,T2;
    toCamera(R1,T1);
    that.toCamera(R2,T2);
    Mat R=R2*R1;
    Mat T=R2*T1+T2;
    return Pose(R,T);
  }

  // so when x --------RT1------> z
  //         x --RT2--> y --RT--> z
  // then, 
  //   z = R1*x+T1
  //   y = R2*x+T2
  //   z = R*y+T
  // therefore,
  //   z = R1*x+T1 = R*y+T = R*(R2*x+T2x)+T
  //         (1)                (2)
  // to be identically (1)=(2), we get 
  //   R = R1*R2.inv() = R1*R2.transpose()
  //   T = T1-R*T2
  Pose operator-(const Pose &that) {
    Mat R1,T1,R2,T2;
    toCamera(R1,T1);
    that.toCamera(R2,T2);
    Mat R2_t;
    transpose(R2,R2_t);
    Mat R=R1*R2_t;
    Mat T=T1-R*T2;
    return Pose(R,T);
  }

  // At first, we have to convert from
  // ros/gazebo world coord to cv/ceres coord.
  // suppose we are just at origin of coordinate,
  // we can translate as 
  //   gazebo(x,y,z) --> cv(-y,-z,x) ... (A)
  //   cv(x,y,z) --> gazebo(z,-x,-y) ... (B)
  // 
  // And then, thanks to 
  //   http://ksimek.github.io/2012/08/22/extrinsic/
  //   ... (C)
  //
  // Note that conversion of eular angles are same
  // as the conversion way of xyz-point coordinate.

  void toCamera(Mat &R,Mat &T) const {
    Mat rot,rvec=(Mat_<double>(3,1) << -o.y,-o.z,o.x); // (A)
    Rodrigues(rvec,rot);
    transpose(rot,R);
    T=-R*(Mat_<double>(3,1) << -p.y,-p.z,p.x); // (A) and (C)
  }
    
  void toCamera(double *cam) const {
    Mat R,rvec,tvec;
    toCamera(R,tvec);
    Rodrigues(R,rvec);
    cam[0]=rvec.at<double>(0,0);
    cam[1]=rvec.at<double>(1,0);
    cam[2]=rvec.at<double>(2,0);
    cam[3]=tvec.at<double>(0,0);
    cam[4]=tvec.at<double>(1,0);
    cam[5]=tvec.at<double>(2,0);
  }
    
  void fromCamera(const Mat &R,const Mat &T) {
    Mat R_t;
    transpose(R,R_t);
    Mat rvec,tvec=-R_t*T; // (C)
    Rodrigues(R_t,rvec);
    o.x=rvec.at<double>(2,0);
    o.y=-rvec.at<double>(0,0);
    o.z=-rvec.at<double>(1,0);
    p.x=tvec.at<double>(2,0);
    p.y=-tvec.at<double>(0,0);
    p.z=-tvec.at<double>(1,0);
    // derived from (B)
  }

  void fromCamera(const double *cam) {
    Mat rvec=(Mat_<double>(3,1) << cam[0],cam[1],cam[2]);
    Mat R,T=(Mat_<double>(3,1) << cam[3],cam[4],cam[5]);
    Rodrigues(rvec,R);
    fromCamera(R,T);
  }
  
  // convert from world pose (gazebo style)
  //           to local pose (gazebo style)
  Point3f world2local(const Point3f &a) const { 
    Point3f b=a-p;
    Mat rvec=(Mat_<double>(3,1) << o.x,o.y,o.z);
    Mat rot,rot_t;
    Rodrigues(rvec,rot);
    transpose(rot,rot_t);
    Mat tvec=rot_t*(Mat_<double>(3,1) << b.x,b.y,b.z);
    b.x=tvec.at<double>(0,0);
    b.y=tvec.at<double>(1,0);
    b.z=tvec.at<double>(2,0);
    return b;
  }

  // convert from local pose (gazebo style)
  //           to world pose (gazebo style)
  Point3f local2world(const Point3f &a) const { 
    Mat rvec=(Mat_<double>(3,1) << o.x,o.y,o.z);
    Mat rot;
    Rodrigues(rvec,rot);
    Mat tvec=rot*(Mat_<double>(3,1) << a.x,a.y,a.z);
    Point3f b=p;
    b.x+=tvec.at<double>(0,0);
    b.y+=tvec.at<double>(1,0);
    b.z+=tvec.at<double>(2,0);
    return b;
  }

  // convert from local pose (cv style)
  //           to world pose (gazebo style)
  Point3f cv2world(const Point3f &a) const { 
    return local2world(Point3f(a.z,-a.x,-a.y)); // (B)
  }

  double distance(const Pose &that) {
    return calcDistance(p,that.p);
  }
};

#endif //#ifndef SLAM_COMMON_H