Get the stereoRectify parameters.

[XanxusCrypto]

Hey thanks for providing the multi-sensor dataset which was approximate to run kinds of SLAM algorithms. I was met with the stereo camera calibration issue when I tried to run ORB-SLAM2 stereo mode with your dataset, as pre-rectified stereo images must be provided. That was to say that I should use the cv::fisheye::stereoRectify function to get the P1, P2, R1, R2 matrixs of the stereo camera. As you can see in the following pictures, I input the K1, K2, D1, D2 matrixs from the sensor.yaml and transform_matrix.yaml file you provided, while the calculated P1, P2 matrixs was wrong. The float number of P2 in row 0 col2 was 3.2546899 which meant the baseline of the T265 stereo camera was 3.25mm, which I searched in the Internet a number of an larger order of magnitude such as 55~80 was normal. So would you like to point out which parameter I set during the stereoRectify process was not proper, or give me the proper parameters to run ORB-SLAM2? Thanks a lot! You can communicate with me directly with e-mail vs09970997@zju.edu.cn :)

[cedrusx]

Hi @XanxusCrypto, I noticed that the translation between the two cameras from your first screenshot is correct (6.39 cm, as in T_LEFT_RIGHT(3,0). But T_RIGHT_LEFT seems to be identical - would that be a problem?). Could you check again the calculations?

[XanxusCrypto]

Hi @XanxusCrypto, I noticed that the translation between the two cameras from your first screenshot is correct (6.39 cm, as in T_LEFT_RIGHT(3,0). But T_RIGHT_LEFT seems to be identical - would that be a problem?). Could you check again the calculations?

Hi thanks for replying! As I got the transform matrix between fisheye1 camera and fisheye2 camera from the transform_matrix.yaml without knowing the relative relationship, while the input parameter of cv::fisheye::stereoRectify should be the transform matrix from the left camera to right camera, thus I tried the T_LEFT_RIGHT matrix and its transpose both. Thus the input matrix T_RIGHT_LEFT may juse be the same as T_LEFT_RIGHT in case.

The rectified stereo images were basically the same whether I used T_LEFT_RIGHT or its transpose, as can be seen in the pictures following by. I used cv::fisheye::stereoRectify(K_l, D_l, K_r, D_r, cv::Size(imgSize), R, t, R_l, R_r, P_l, P_r, Q, CALIB_ZERO_DISPARITY, cv::Size(imgSize)); and cv::fisheye::initUndistortRectifyMap(K_l, D_l, R_l, P_l, rowRange(0, 3), colRange(0, 3), cv::Size(imgSize), CV_32F, M1l, M1r); as well as cv::remap.

[cedrusx]

I suppose your question is about the value of 3.2546899 in P_r(0,3). I am not familiar with cv::fisheye::stereoRectify. I checked the document and found that this value means Tx*f, where Tx is the horizontal component of the baseline, and f is the recified focal, 50.872 in this case. So we have Tx = 3.2546899 / 50.872 = 0.063978, consistent with the value from transform_matrix.yaml.

[XanxusCrypto]

I suppose your question is about the value of 3.2546899 in P_r(0,3). I am not familiar with cv::fisheye::stereoRectify. I checked the document and found that this value means Tx*f, where Tx is the horizontal component of the baseline, and f is the recified focal, 50.872 in this case. So we have Tx = 3.2546899 / 50.872 = 0.063978, consistent with the value from transform_matrix.yaml.

Yes that was what I could not figure out, for which the rectified focal and thus the camera baseline (namely the value of 3.2546899 in P_r(0,3) ) were both abnormal. In a result, it can be seen that the subjects in the recitified stereo images were indeed regular, but the viewpoint was draged away from the center. I also use the simple function cv::fisheye::undistortImage, which only need K and D matrix of the fisheye camera, but the result was also awful. Would you plz take a try to rectify the stereo fisheye camera? Below is my code.

cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r, Q;
fsSettings["LEFT.K"] >> K_l;
fsSettings["RIGHT.K"] >> K_r;
fsSettings["LEFT.D"] >> D_l;
fsSettings["RIGHT.D"] >> D_r;

cv::Mat T_LEFT_RIGHT = (cv::Mat_<double>(4, 4) << 9.9997103351978955e-01, -3.0859078732664710e-03, 6.9576787768359741e-03, 6.3976511359200003e-02,
                                           3.1121161181976121e-03, 9.9998809175217129e-01, -3.7591338254934705e-03, 1.4826713595499999e-04,
                                           -6.9459955824040347e-03, 3.7806780408845384e-03, 9.9996872932053249e-01, -3.9846837171399999e-04,
                                           0.0, 0.0, 0.0, 1.0);
// fsSettings["LEFT.RIGHT"] >> T_RIGHT_LEFT;

cv::Mat K1, K2, D1, D2;
K_l.rowRange(0, 3).colRange(0, 3).copyTo(K1);
K_r.rowRange(0, 3).colRange(0, 3).copyTo(K2);
D_l.row(0).colRange(0, 4).copyTo(D1);
D_r.row(0).colRange(0, 4).copyTo(D2);

cv::Mat T_RIGHT_LEFT(4, 4, CV_64FC1);
T_RIGHT_LEFT = T_LEFT_RIGHT.inv();
// T_RIGHT_LEFT = T_LEFT_RIGHT;
// std::cout<<"T_RIGHT_LEFT"<<std::endl<<T_RIGHT_LEFT<<std::endl;
// cout.flags(ios::fixed);
// cout.precision(14);

cv::Mat R(3, 3, CV_64FC1); 
T_RIGHT_LEFT.rowRange(0, 3).colRange(0, 3).copyTo(R);
cv::Mat t(3, 1, CV_64FC1); 
T_RIGHT_LEFT.rowRange(0, 3).col(3).copyTo(t);
std::cout<<"R"<<std::endl<<R<<std::endl;
std::cout<<"t"<<std::endl<<t<<std::endl;

std::cout<<"K1"<<std::endl<<K1<<std::endl;
std::cout<<"K2"<<std::endl<<K2<<std::endl;
std::cout<<"D1"<<std::endl<<D1<<std::endl;
std::cout<<"D2"<<std::endl<<D2<<std::endl;

cv::fisheye::stereoRectify(K1, D1, K2, D2, cv::Size(cols_l, rows_l), R, t, R_l, R_r, P_l, P_r, Q, CALIB_ZERO_DISPARITY, cv::Size(cols_l, rows_l));
// stereoRectify(K_l, D_l, K_r, D_r, cv::Size(cols_l, rows_l), R, t, R_l, R_r, P_l, P_r, Q, CALIB_ZERO_DISPARITY, 0);

std::cout<<"R_l"<<std::endl<<R_l<<std::endl;
std::cout<<"R_r"<<std::endl<<R_r<<std::endl;
std::cout<<"P_l"<<std::endl<<P_l<<std::endl;
std::cout<<"P_r"<<std::endl<<P_r<<std::endl;

cv::Mat M1l,M2l,M1r,M2r;
cv::fisheye::initUndistortRectifyMap(K1,D1,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
cv::fisheye::initUndistortRectifyMap(K2,D2,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);
// cv::initUndistortRectifyMap(K1,D1,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
// cv::initUndistortRectifyMap(K2,D2,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);

cv::Mat letfImg = cv::imread("/home/sunny/XDataBenchMark/OpenLORIS/office1-3/fisheye1/1560000110.947943.png",CV_LOAD_IMAGE_UNCHANGED);
cv::Mat RightImg = cv::imread("/home/sunny/XDataBenchMark/OpenLORIS/office1-3/fisheye2/1560000110.947943.png",CV_LOAD_IMAGE_UNCHANGED);

cv::Mat canvas(letfImg.rows, letfImg.cols * 2, CV_8UC1);
cv::Mat canvas_left = canvas(cv::Rect(0, 0, letfImg.cols, letfImg.rows));
cv::Mat canvas_right = canvas(cv::Rect(letfImg.cols, 0, letfImg.cols, letfImg.rows));

letfImg.copyTo(canvas_left);
RightImg.copyTo(canvas_right);

cv::namedWindow("canvas", CV_WINDOW_AUTOSIZE);
cv::imshow("canvas", canvas);
cv::waitKey(0);

cv::Mat leftImageUndistorted, rightImageUndistorted;
// cv::undistort(letfImg, leftImageUndistorted, K1, D1);
// cv::undistort(RightImg, rightImageUndistorted, K2, D2);
cv::fisheye::undistortImage(letfImg, leftImageUndistorted, K1, D1);
cv::fisheye::undistortImage(RightImg, rightImageUndistorted, K2, D2);
leftImageUndistorted.copyTo(canvas_left);
rightImageUndistorted.copyTo(canvas_right);

cv::imshow("canvas", canvas);
cv::waitKey(0);

cv::Mat imLeft;
cv::Mat imRight;
cv::remap(letfImg,imLeft,M1l,M2l,cv::INTER_LINEAR);
cv::remap(RightImg,imRight,M1r,M2r,cv::INTER_LINEAR);

imLeft.copyTo(canvas_left);
imRight.copyTo(canvas_right);

cv::imshow("canvas", canvas);
cv::waitKey(0);

[cedrusx]

Hi @XanxusCrypto , the value of P_r(0,3) does not indicate a camera baseline. Please check again the document and my previous comment.

[XanxusCrypto]

Hi @XanxusCrypto , the value of P_r(0,3) does not indicate a camera baseline. Please check again the document and my previous comment.

Thanks for replying, I got that and the value is the camera.bf value in the ORB-SLAM2 config file which meant the product of focal length and camera baseline. Meanwhile the camera.bf and corresponding recitified fx, fy, cx, fy of stereo cameras were needed as input parameters for SLAM and other pipelines, while it seen that the rectified stereo images were abnormal and the rectified stereo instrintics (fx = 51) were distinct from original ones(fx = 285). By now I was just wondering how to get the right rectified stereo images so as to run ORB-SLAM2 algorithms, and other pipelines. Feel sorry for bothering a lot.

[LeatherWang]

@XanxusCrypto You should fix optical axid disalign before running stereo rectify

[XanxusCrypto]

@LeatherWang I did not how to align the optical axis of the two cameras.. While I could not see difference for the given fisheye instrinsic parameters from that of EurocDataset.yaml, which I stereoRectify the pinhole stereo camera images successfully. I tried to used the individual K and D matrix to undistort the left image with cv::fisheye::undistortImage and K [284.981, 0.0, 425.244, 0.0, 286.102, 398.468, 0.0, 0.0, 1.0] D [-0.0073047, 0.0434999, -0.04128304, 0.007652460, 0.0], the resulting rectified image was also totally wrong.

[cedrusx]

Hi @XanxusCrypto, I'm sorry that I may not be able to give a lot of help immediately since I do not have any experience on dealing with fisheye distortions. But I would like to understand what is happening. Would you like to verify your code with some well-proven datasets such as TUM VI?

[XanxusCrypto]

@cedrusx Thanks for providing the fisheye dataset, which I was just intended to test the rectify code! I would try some other dataset and look in-depth what's going wrong.

[dinarkino]

@XanxusCrypto hi! Thank you for the parameters. I'm trying to run openvslam using stereo images from openloris dataset. There are almost the same parameters settings as in ORB_SLAM2. Did you finally find appropriate parameters?

I'm also wondering how do you handle data so that ORB_SLAM2 could run it? Now I'm trying to use euroc examples since it's also stereo, but there is a problem with image names in openloris which use tum type of timestamps.

Actually @cedrusx that would be cool if the rectification and projection matrixes would be available in configs as it's done, for example, in Kaist dataset.

[cedrusx]

@XanxusCrypto hi! Thank you for the parameters. I'm trying to run openvslam using stereo images from openloris dataset. Parameters settings there are almost the same as in ORB_SLAM2. Did you finally find appropriate parameters?

I'm also wondering how do you handle data so that ORB_SLAM2 could run it? Now I'm trying to use euroc examples since it's also stereo, but there is a problem with image names in openloris which use tum type of timestamps.

Actually @cedrusx that would be cool if the rectification and projection matrixes would be available in configs as it's done, for example, in Kaist dataset.

Hi @dinarkino thank you for the suggestion! It would definitely be good to have those config. Unfortunately I may not have enough bandwidth recently to learn how to do that :( Can you help on that? Let me know if you need any extra data.

[dinarkino]

So far I got such a result using parameters from @XanxusCrypto message for cafe1-1 sequence.

Seems like something wrong with that. I'm not very familiar with cv::fisheye::stereoRectify but I will try to figure out something later. If someone already used stereo images from openloris dataset, please share parameters for rectification.

Parameters that I used now (from openvslam config file):

Camera.name: "openloris stereo"
Camera.setup: "stereo"
Camera.model: "perspective"

# new "rectified" matrices is the first three cols of the projection matrix which is calculated with cv::stereoRectify()
# e.g. fx = P1[0][0] or P2[0][0], cx = P1[0][2] or P2[0][2]
#      fy = P1[1][1] or P2[1][1], cy = P1[1][2] or P2[1][2]

Camera.fx: 50.87207738052201
Camera.fy: 50.87207738052201
Camera.cx: 457.7320908130409
Camera.cy: 23.32796223258296

# there is no distortion after stereo rectification

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.0

# focal_x_baseline is -P2[0][3] which is calculated with cv::stereoRectify()

Camera.fps: 30
Camera.cols: 848
Camera.rows: 800
Camera.focal_x_baseline: 3.254689902969693

Camera.color_order: "Gray"

#======================#
# Stereo Rectification #
#======================#

# original intrinsic parameters (K, D) and stereo-recitification parameters (R)
# matrices (K, R) are written in row-major order
# StereoRectifier.model is camera model before rectification

StereoRectifier.model: "fisheye"
StereoRectifier.K_left: [284.98089599609375, 0.0, 425.24438476562500, 0.0, 286.10238647460938, 398.46759033203125, 0.0, 0.0, 1.0]
StereoRectifier.D_left: [-0.0073047108016908169, 0.043499931693077087, -0.041283041238784790, 0.0076524601317942142]
StereoRectifier.R_left: [0.9999994266444558, -0.0007919408315897027, 0.0007207915642510241, 0.0007932975104164358, 0.9999979108520046, -0.001883871196859428, -0.0007192981438885341, 0.001884441918884863, 0.9999979657423482]
StereoRectifier.K_right: [284.81259155273438, 0.0, 427.66159057617188, 0.0, 285.97601318359375, 397.12341308593750, 0.0, 0.0, 1.0]
StereoRectifier.D_right: [-0.0063794981688261032, 0.041455611586570740, -0.039464488625526428, 0.0069808149710297585]
StereoRectifier.R_right: [0.99997791907347, 0.002317473380928515, -0.006228216648564348, -0.002305734265605451, 0.9999955531869615, 0.001891347614549239, 0.00623257210060014, -0.001876945239301329, 0.9999788158364054]

[mirellameelo]

@XanxusCrypto Hi! I'm facing just the same. =/ Did you succeed in rectifying the images? Tks in advance.

[bot-lin]

I am using Realsense T265 and its ROS wrapper. I got the parameters of the camera such as K, R, D, Projection matrix from the ROS topic /camera/fisheye1 (2)/camera_info. Then I filled config file mentioned by @dinarkino with these matrices. Sadly it was not working for me.

header: seq: 13855 stamp: secs: 1618318847 nsecs: 867653608 frame_id: "camera_fisheye2_optical_frame" height: 800 width: 848 distortion_model: "equidistant" D: [-0.01056853961199522, 0.049965810030698776, -0.04700354114174843, 0.009185051545500755, 0.0] K: [285.4751892089844, 0.0, 423.0504150390625, 0.0, 285.3926086425781, 406.177001953125, 0.0, 0.0, 1.0] R: [0.9999827742576599, -0.0005485129076987505, -0.005836081225425005, 0.0005396709893830121, 0.9999986886978149, -0.0015165233053267002, 0.005836906377226114, 0.001513347728177905, 0.9999817609786987] P: [287.9395751953125, 0.4836355447769165, 421.37664794921875, 18.44516944885254, 2.5248353481292725, 286.0069274902344, 405.7367858886719, 0.0, 0.005836906377226114, 0.001513347728177905, 0.9999817609786987, 0.0] binning_x: 0 binning_y: 0 roi: x_offset: 0 y_offset: 0 height: 0 width: 0 do_rectify: False