/************************* * * 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 }; }