/*************************
*
* init the starting position of the robot;
*/
void visualOdometry::initPosition(){
cv::Mat t_mat(3,1,CV_64FC1),r_mat(3,1,CV_64FC1);
//camera translation;
t_mat.ptr<double>(0)[0] = -0.6832;
t_mat.ptr<double>(1)[0] = 2.6909;
t_mat.ptr<double>(2)[0] = 1.7373;
tf::Quaternion quaternion(0.0003,0.8617,-0.5072,-0.0145);
tf::Matrix3x3 mat_quaternion(quaternion);
double x,y,z;
mat_quaternion.getEulerZYX(z,y,x);
r_mat.ptr<double>(0)[0] = x;
r_mat.ptr<double>(1)[0] = y;
r_mat.ptr<double>(2)[0] = z;
cv::Mat rotation_cv;
cv::Rodrigues(r_mat,rotation_cv);
//trans cv to eigen;
Eigen::Matrix3d rotation_eigen_temp;
cv::cv2eigen(rotation_cv,rotation_eigen_temp);
Eigen::AngleAxisd rotation_eigen(rotation_eigen_temp);
//assign rotation
pose_camera_ = Eigen::Isometry3d::Identity();
pose_camera_ = rotation_eigen;
//assign translation
pose_camera_(0,3) = t_mat.ptr<double>(0)[0];
pose_camera_(1,3) = t_mat.ptr<double>(1)[0];
pose_camera_(2,3) = t_mat.ptr<double>(2)[0];
}
/*
* View Matrix:
* RX RY RZ -PX
* UX UY UZ -PY
* -FX -FY -FZ -PZ
* 0 0 0 1
*
* First, create the translation matrix (only the position 'P' values).
* Then construct the rotation matrix (upper-left 3x3).
* The view matrix is V = R * T.
* Note that 'up' must be udpated as the camera rotates.
*/
m4 look_at(v3 pos, v3 t_pos, v3 up) {
m4 t_mat = id4();
// Apply translation.
t_mat.m[3] = -pos.v[0];
t_mat.m[7] = -pos.v[1];
t_mat.m[11] = -pos.v[2];
// Find right/forwards/up rotational vectors.
// Distance from camera to target
v3 dist = t_pos - pos;
// Forward vector is pointing towards the target.
v3 f = normalize(dist);
// R is F x U.
v3 r = cross(f, up);
// Recalculate up as R x F.
v3 u = cross(r, f);
// Create rotation matrix.
m4 r_mat(r.v[0], r.v[1], r.v[2], 0,
u.v[0], u.v[1], u.v[2], 0,
-f.v[0], -f.v[1], -f.v[2], 0,
0, 0, 0, 1);
// R * T to get the view matrix.
m4 v_mat = r_mat * t_mat;
return v_mat;
}
CalibrationParams
CalibrationParams::createCalibrationParams(std::string serial, cv::FileNode node) {
cv::Mat camera_matrix (cv::Mat::eye(3, 3, CV_64FC1));
cv::Mat dist_coeffs (5, 1, CV_64FC1, cv::Scalar::all(0));
cv::Mat r_mat (3, 3, CV_64FC1, cv::Scalar::all(0));
cv::Mat t (3, 1, CV_64FC1, cv::Scalar::all(0));
node["camera_matrix"] >> camera_matrix;
node["distortion_coefficients"] >> dist_coeffs;
node["rotation"] >> r_mat;
node["translation"] >> t;
float fx = camera_matrix.ptr<double>(0)[0];
float fy = camera_matrix.ptr<double>(0)[4];
float cx = camera_matrix.ptr<double>(0)[2];
float cy = camera_matrix.ptr<double>(0)[5];
Eigen::Matrix4f tmp_mat = Eigen::Matrix4f::Identity();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
tmp_mat(i, j) = r_mat.at<double>(i, j);
}
}
for (int i = 0; i < 3; i++) {
tmp_mat(i, 3) = t.at<double>(i);
}
return {
serial,
camera_matrix,
dist_coeffs,
r_mat,
t,
tmp_mat.inverse(),
fx,
fy,
cx,
cy
};
}
