Skip to content

Instantly share code, notes, and snippets.

@chiral
Created April 10, 2016 15:14
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save chiral/1e0e1ee1c45705de9b87538fd2e42bb4 to your computer and use it in GitHub Desktop.
Save chiral/1e0e1ee1c45705de9b87538fd2e42bb4 to your computer and use it in GitHub Desktop.
code example of AIBOX DroneBrain
#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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment