OpenCV Q&A Forum - RSS feedhttp://answers.opencv.org/questions/OpenCV answersenCopyright <a href="http://www.opencv.org">OpenCV foundation</a>, 2012-2018.Fri, 07 Dec 2018 06:35:46 -0600Calculate robot coordinates from measured chessbaord corners ( hand-eye calibration)http://answers.opencv.org/question/204910/calculate-robot-coordinates-from-measured-chessbaord-corners-hand-eye-calibration/Hello guys,
for my project I am trying to calibrate my depth camera with my robot-arm (hand-eye calibration). Therefore, I measured 8 corners of the cheassboard and got 8 pixel vectors and their corresponding robot 3D coordinates (using the gripper to point exactly at the corner). Then, I tried to calculate the transformation matrix. But the result is way off. Where's the mistake? Thank you very much!
Edit: Added the camera matrix
----------
img_pts = np.array(((237., 325.),
(242., 245.),
(248., 164.),
(318., 330.),
(322., 250.),
(328., 170.),
(398., 336.),
(403., 255.)), dtype=np.float32)
objectPoints = np.array(((-437., -491., -259.),
(-402., -457., -259.),
(-360., -421., -259.),
(-393., -534., -259.),
(-362., -504., -259.),
(-332., -476., -259.),
(-298., -511., -259.),
(-334., -546., -259.)), dtype=np.float32)
camera_matrix = np.array(((542.5860286838929, 0., 310.4749867256299),
(0., 544.2396851407029, 246.7577402755397),
(0., 0., 1.)), dtype=np.float32)
distortion = np.array([0.03056762860315778,
-0.1824835906443329,
-0.0007936359893356936,
-0.001735795279032343,
0])
ret, rvec, tvec = cv2.solvePnP(objectPoints, img_pts, camera_matrix, distortion)
rmtx, _ = cv2.Rodrigues(rvec)
T_gripper_cam = np.array([[rmtx[0][0], rmtx[0][1], rmtx[0][2], tvec[0]],
[rmtx[1][0], rmtx[1][1], rmtx[1][2], tvec[1]],
[rmtx[2][0], rmtx[2][1], rmtx[2][2], tvec[2]],
[0.0, 0.0, 0.0, 1.0]])
T_cam_gripper = np.linalg.inv(T_gripper_cam)
print(T_cam_gripper)
p = np.array((237., 325., 0.), dtype=np.float32)
p_new = np.array([p[0], p[1], p[2], 1])
objectPoint = np.dot(p_new, T_gripper_cam)[:3]
print(objectPoint)
# should be (-437., -491., -259.)
# --> actual = [ 33.57 -395.62 -64.46]
------------------------------------- Edit ----------------------------------------------------------
I think I might be on to something. According to this:
[Stackoverflow](https://stackoverflow.com/questions/12299870/computing-x-y-coordinate-3d-from-image-point)
I have to calculate the scaling factor. So i tried to do this in python:
uv = np.array(([237.], [325.], [1.]), dtype=np.float32)
rotInv = np.linalg.inv(rmtx)
camMatInv = np.linalg.inv(camera_matrix)
leftSideMat = np.dot(rotInv, np.dot(camMatInv, uv))
rightSideMat = np.dot(rotInv, tvec)
s = (295 + leftSideMat[2:] / rightSideMat[2:])
temp = (s * np.dot(camMatInv, uv))
tempSub = np.array(temp - tvec)
print(np.dot(rotInv, tempSub))
# should be (-437., -491., -259.)
# --> actual = [ 437.2 -501.3 -266.2]
Looks like I am pretty close. But its still way off in the Y direction. Is this the correct approach? Thank you!Mysterion46Fri, 07 Dec 2018 06:35:46 -0600http://answers.opencv.org/question/204910/Augmented reality with Aruco and SceneKithttp://answers.opencv.org/question/199746/augmented-reality-with-aruco-and-scenekit/ I'm trying to make augmented reality demo project with simple 3d object in the center of marker. I need to make it with `OpenCV` and `SceneKit`.
My steps are:
- I obtain the corners of marker using `Aruco`
- with `cv::solvePnP` get the `tvec` and `rvec`.
- convert the `tvec` and `rvec` from `OpenCv's Coordinate System` to the `SceneKit Coordinate System`.
- apply the converted rotation and translation to the camera node.
The Problem is:
- The object is not centered on the marker. Rotation of object looks good. But is not positioned where it should be.
SolvePnp code:
cv::Mat intrinMat(3,3,cv::DataType<double>::type);
//From ARKit (ARFrame camera.intrinsics) - iphone 6s plus
intrinMat.at<double>(0,0) = 1662.49;
intrinMat.at<double>(0,1) = 0.0;
intrinMat.at<double>(0,2) = 0.0;
intrinMat.at<double>(1,0) = 0.0;
intrinMat.at<double>(1,1) = 1662.49;
intrinMat.at<double>(1,2) = 0.0;
intrinMat.at<double>(2,0) = 960.0 / 2;
intrinMat.at<double>(2,1) = 540.0 / 2;
intrinMat.at<double>(2,2) = 0.0;
double marker_dim = 3;
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
CVPixelBufferLockBaseAddress(pixelBuffer, 0);
void *baseaddress = CVPixelBufferGetBaseAddressOfPlane(pixelBuffer, 0);
CGFloat width = CVPixelBufferGetWidth(pixelBuffer);
CGFloat height = CVPixelBufferGetHeight(pixelBuffer);
cv::Mat mat(height, width, CV_8UC1, baseaddress, 0); //CV_8UC1
cv::rotate(mat, mat, cv::ROTATE_90_CLOCKWISE);
std::vector<int> ids;
std::vector<std::vector<cv::Point2f>> corners;
cv::aruco::detectMarkers(mat,dictionary,corners,ids);
if(ids.size() > 0) {
cv::Mat colorMat;
cv::cvtColor(mat, colorMat, CV_GRAY2RGB);
cv::aruco::drawDetectedMarkers(colorMat, corners, ids, cv::Scalar(0,255,24));
cv::Mat distCoeffs = cv::Mat::zeros(8, 1, cv::DataType<double>::type); //zero out distortion for now
//MARK: solvepnp
std::vector<cv::Point3f> object_points;
object_points = {cv::Point3f(-marker_dim , marker_dim , 0),
cv::Point3f(marker_dim , marker_dim , 0),
cv::Point3f(marker_dim , -marker_dim , 0),
cv::Point3f(-marker_dim , -marker_dim , 0)};
std::vector<cv::Point_<float>> image_points = std::vector<cv::Point2f>{corners[0][0], corners[0][1], corners[0][2], corners[0][3]};
std::cout << "object points: " << object_points << std::endl;
std::cout << "image points: " << image_points << std::endl;
cv::Mat rvec, tvec;
cv::solvePnP(object_points, image_points, intrinMat, distCoeffs, rvec, tvec);
cv::aruco::drawAxis(colorMat, intrinMat, distCoeffs, rvec, tvec, 3);
cv::Mat rotation, transform_matrix;
cv::Mat RotX(3, 3, cv::DataType<double>::type);
cv::setIdentity(RotX);
RotX.at<double>(4) = -1; //cos(180) = -1
RotX.at<double>(8) = -1;
cv::Mat R;
cv::Rodrigues(rvec, R);
std::cout << "rvecs: " << rvec << std::endl;
std::cout << "cv::Rodrigues(rvecs, R);: " << R << std::endl;
R = R.t(); // rotation of inverse
std::cout << "R = R.t() : " << R << std::endl;
cv::Mat rvecConverted;
Rodrigues(R, rvecConverted); //
std::cout << "rvec in world coords:\n" << rvecConverted << std::endl;
rvecConverted = RotX * rvecConverted;
std::cout << "rvec scenekit :\n" << rvecConverted << std::endl;
Rodrigues(rvecConverted, rotation);
std::cout << "-R: " << -R << std::endl;
std::cout << "tvec: " << tvec << std::endl;
cv::Mat tvecConverted = -R * tvec;
std::cout << "tvec in world coords:\n" << tvecConverted << std::endl;
tvecConverted = RotX * tvecConverted;
std::cout << "tvec scenekit :\n" << tvecConverted << std::endl;
SCNVector4 rotationVector = SCNVector4Make(rvecConverted.at<double>(0), rvecConverted.at<double>(1), rvecConverted.at<double>(2), norm(rvecConverted));
SCNVector3 translationVector = SCNVector3Make(tvecConverted.at<double>(0), tvecConverted.at<double>(1), tvecConverted.at<double>(2));
std::cout << "rotation :\n" << rotation << std::endl;
transform_matrix.create(4, 4, CV_64FC1);
transform_matrix( cv::Range(0,3), cv::Range(0,3) ) = rotation * 1;
transform_matrix.at<double>(0, 3) = tvecConverted.at<double>(0,0);
transform_matrix.at<double>(1, 3) = tvecConverted.at<double>(1,0);
transform_matrix.at<double>(2, 3) = tvecConverted.at<double>(2,0);
transform_matrix.at<double>(3, 3) = 1;
TransformModel *model = [TransformModel new];
model.rotationVector = rotationVector;
model.translationVector = translationVector;
return model;
}
swift code:
func initSceneKit() {
let scene = SCNScene()
cameraNode = SCNNode()
let camera = SCNCamera()
camera.zFar = 1000
camera.zNear = 0.1
cameraNode.camera = camera
scene.rootNode.addChildNode(cameraNode)
let scnView = sceneView!
scnView.scene = scene
scnView.autoenablesDefaultLighting = true
scnView.backgroundColor = UIColor.clear
let box = SCNBox(width: 10, height: 10 , length: 10, chamferRadius: 0)
boxNode = SCNNode(geometry: box)
boxNode.position = SCNVector3(0,0,0)
scene.rootNode.addChildNode(boxNode)
sceneView.pointOfView = cameraNode
}
func initCamera() {
let device = AVCaptureDevice.default(AVCaptureDevice.DeviceType.builtInWideAngleCamera, for: .video, position: AVCaptureDevice.Position.back)
let deviceInput = try! AVCaptureDeviceInput(device: device!)
self.session = AVCaptureSession()
self.session.sessionPreset = AVCaptureSession.Preset.iFrame960x540
self.session.addInput(deviceInput)
let sessionOutput: AVCaptureVideoDataOutput = AVCaptureVideoDataOutput()
let outputQueue = DispatchQueue(label: "VideoDataOutputQueue", attributes: [])
sessionOutput.setSampleBufferDelegate(self, queue: outputQueue)
self.session.addOutput(sessionOutput)
self.previewLayer = AVCaptureVideoPreviewLayer(session: self.session)
self.previewLayer.backgroundColor = UIColor.black.cgColor
self.previewLayer.videoGravity = AVLayerVideoGravity.resizeAspect
self.previewView.layer.addSublayer(self.previewLayer)
self.session.startRunning()
view.setNeedsLayout()
}
func captureOutput(_ output: AVCaptureOutput, didOutput sampleBuffer: CMSampleBuffer, from connection: AVCaptureConnection) {
let pixelBuffer: CVPixelBuffer = CMSampleBufferGetImageBuffer(sampleBuffer)!
//QR detection
guard let transQR = OpenCVWrapper.arucoTransformMatrix(from: pixelBuffer) else {
return
}
DispatchQueue.main.async(execute: {
self.setCameraMatrix(transQR)
})
}
func setCameraMatrix(_ transformModel: TransformModel) {
cameraNode.rotation = transformModel.rotationVector
cameraNode.position = transformModel.translationVector
// cameraNode.transform = transformModel.transform
}
[image of my result](https://i.stack.imgur.com/Calxx.jpg)
Repo on github with my project: [https://github.com/danilovdorin/ArucoAugmentedReality](https://github.com/danilovdorin/ArucoAugmentedReality)dorindanilovWed, 19 Sep 2018 15:28:20 -0500http://answers.opencv.org/question/199746/Rotation vector interpretationhttp://answers.opencv.org/question/197981/rotation-vector-interpretation/I use opencv cv2.solvePnP() function to calculate rotation and translation vectors. Rotation is returned as rvec [vector with 3DOF]. I would like to ask for help with interpreting the rvec.
As far as I understand rvec = the rotation vector representation:
- the rotation vector is the axis of the rotation
- the length of rotation vector is the rotation angle θ in radians [around axis, so rotation vector]
Rvec returned by solvePnP:
rvec =
[[-1.5147142 ]
[ 0.11365167]
[ 0.10590861]]
Then:
angle_around_rvec = sqrt(-1.5147142^2 + 0.11365167^2 + 0.10590861^2) [rad] = 1.52266 [rad] = 1.52266*180/3.14 [deg] = 87.286 [deg]
**1. Does 3 rvec components correspond to world coordinates? Or what are these directions?**
**2. Can I interpret the vector components as separate rotation angles in radians around components directions?**
My rvec components interpretation:
angle_around_X = -1.5147142 [rad] = -1.5147*180/3.14 [deg] = -86.83 [deg]
angle_around_Y = 0.11365167 [rad] = 0.11365167*180/3.14 [deg] = 6.52 [deg]
angle_around_Z = 0.10590861 [rad] = 0.10590861*180/3.14 [deg] = 6.07 [deg]
My usecase:
I have coordinates of four image points. I know the coordinates of these points in the real world. I know camera intrinsic matrix. I use PnP3 to get rotation and translation vector. From rotation matrix, I would like to find out what are the angles around fixed global/world axes: X, Y, Z. I am NOT interested in Euler angles. I want to find out how an object is being rotated around the fixed world coordinates (not it's own coordinate system).
I would really appreciate your help. I feel lost in rotation.
Thank you in advance.dziadygeThu, 23 Aug 2018 13:55:50 -0500http://answers.opencv.org/question/197981/Strange ArUco behavior / OpenCV SolvePnphttp://answers.opencv.org/question/190853/strange-aruco-behavior-opencv-solvepnp/Hi There,
I tested the accuracy of ArUco markers in severeal distances and used for that a board with a not rotated marker, several rotated marker on their z-Axis (known rotation) and several more not rotated marker. All markers are measured in their rotation in relation to the first not rotated marker.
Now I calculate the transformation (with quaternions) from the marker of interest in the reference marker. The output for my accuracy is the angle theta from the axis angles. The strange thing is, that the rotation error of the rotated (5°, 20°, 30°, 45°, 90° 180°) markers is small (max. 1°) and the error of the not roted marker (0°) is large (>2°).
I do the subpixel corner refinement and the detected markers looks fine for me. I can exclude the error of the camera, because changing positions (near to the center of the image or not) does not change the accuracy. As well I switched the IDs.
How can it be, that rotated marker are more accurate then non roated marker. Could it be a singularity on the detection or numeric errors?
Thank you for your help!
Sarahsarah1802Fri, 04 May 2018 02:31:17 -0500http://answers.opencv.org/question/190853/Output from solvePnP doesn't match projectPointshttp://answers.opencv.org/question/180064/output-from-solvepnp-doesnt-match-projectpoints/ I get strange data from solvePnP, so I tried to check it with projectPoints:
retval, rvec, tvec=cv2.solvePnP(opts, ipts, mtx, dist, flags=cv2.SOLVEPNP_ITERATIVE)
print(retval,rvec,tvec)
proj, jac = cv2.projectPoints(opts, rvec, tvec, mtx, dist)
print(proj,ipts)
here opts are 3d points with z=0, detected on this picture:
[![enter image description here][1]][1]
And ipts are taken from this pic (only part of picture here):
[![enter image description here][2]][2]
I've checked points themselves (detected with SIFT, points are detected correctly and pairing in a right way).
Now I want to test if rvec and tvec, found by SolvePnP is correct, so I invoke cv2.projectPoint to test if 3d points are projected to the image points. And here is what I have:
[![enter image description here][3]][3]
So I see that projected points lie outside of image, having y<0.
(retval from solvePnP is true)
This is distortion matrix dist:
1.6324642475694839e+02 -2.1480843988631259e+04 -3.4969507980045117e-01 7.9693609309756430e-01 -4.0684056606034986e+01
This is mtx:
6.4154558230601404e+04 0. 1.2973531562160772e+03
0. 9.8908265814965678e+04 9.5760834379036123e+02
0. 0. 1.
This is opts:
[[ 1708.74987793 1138.92041016 0. ]
[ 1708.74987793 1138.92041016 0. ]
[ 1708.74987793 1138.92041016 0. ]
[ 1708.74987793 1138.92041016 0. ]
[ 1708.74987793 1138.92041016 0. ]
[ 1708.74987793 1138.92041016 0. ]
[ 1708.74987793 1138.92041016 0. ]
[ 1984.09973145 1069.31677246 0. ]
[ 1984.09973145 1069.31677246 0. ]
[ 1908.19396973 1200.05529785 0. ]
[ 1994.56677246 1286.16516113 0. ]
[ 1994.56677246 1286.16516113 0. ]
[ 1806.82177734 1058.06872559 0. ]
[ 1925.55639648 1077.33703613 0. ]
[ 1998.30627441 1115.51647949 0. ]
[ 1998.30627441 1115.51647949 0. ]
[ 1998.30627441 1115.51647949 0. ]
[ 2013.79003906 1168.08728027 0. ]
[ 1972.93457031 1234.92614746 0. ]
[ 2029.11364746 1220.234375 0. ]]
This is ipts:
[[ 71.6125946 11.61344719]
[ 116.60684967 71.6068573 ]
[ 116.60684967 71.6068573 ]
[ 101.60684967 86.60684967]
[ 101.60684967 86.60684967]
[ 116.60684967 101.6068573 ]
[ 116.60684967 101.6068573 ]
[ 112.37421417 53.40462112]
[ 112.37421417 53.40462112]
[ 83.76233673 84.36077118]
[ 98.45358276 112.38414764]
[ 98.45358276 112.38414764]
[ 67.2594223 38.04878998]
[ 96.85155487 51.85028076]
[ 112.26165009 67.25630188]
[ 112.26165009 67.25630188]
[ 112.26165009 67.25630188]
[ 112.24694061 82.24401855]
[ 96.82528687 97.66513824]
[ 112.2511673 97.25905609]]
rvec = [[-0.21890167] [-0.86241377] [ 0.96051463]]
tvec = [[ 239.04461181] [-2165.99539286] [-1700.61539107]]
[1]: https://i.stack.imgur.com/BJyhp.png
[2]: https://i.stack.imgur.com/7UFlf.png
[3]: https://i.stack.imgur.com/41lBh.png
[4]: https://i.stack.imgur.com/YbqK3.png
[5]: https://i.stack.imgur.com/Kn2hb.png
stiv-yakovenkoSat, 09 Dec 2017 02:18:45 -0600http://answers.opencv.org/question/180064/How to verify the accuracy of solvePnP return values?http://answers.opencv.org/question/149759/how-to-verify-the-accuracy-of-solvepnp-return-values/Hello , I have used `solvePnP` to find the Pose of object and I am getting some results for `rvet` and `tvet`. Now I wanted to know how accurate they are.
How to I compute the accuracy of retunred values of solvePnP?
1 method I found was re-projection error.
But is there any way to **generate test cases**:
**Data need to generate as test cases:**
(image points, object points)
(expected rvet, expected tvet)
**Result:**
(computed rvet, computed tvet) - return values from solvePnP with each test case that we generated.
And now comparing the `((expected rvet, expected tvet)` and `(computed rvet, computed tvet)` to measure the accuracy for different flags available for solvePnP.
**Are there any ways/software/tools that helps me to generate accurate test cases.**(Varieties of test cases may include nose environment, distance of object from camera varies, planar object points, non co-planar object points .. etc) ?
djkpAFri, 19 May 2017 01:34:36 -0500http://answers.opencv.org/question/149759/OpenCV Error: Assertion failed in undistort.cpp at line 293http://answers.opencv.org/question/149339/opencv-error-assertion-failed-in-undistortcpp-at-line-293/ OpenCV Error:
>Assertion failed (CV_IS_MAT(_src) && CV_IS_MAT(_dst) && (_src->rows == 1 || _src->cols == 1) && (_dst->rows == 1 || _dst->cols == 1) && _src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 && (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) && (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2)) in cvUndistortPoints, file /home/javvaji/opencv-3.2.0/modules/imgproc/src/undistort.cpp, line 293
retval, rvec, tvec = cv2.solvePnP(cam.object_points, cam.image_points, cam.camera_matrix, cam.dist_coefficients, None, None, False, cv2.SOLVEPNP_P3P)
Parameter values: (all are numpy arrays)
Image points:
[[ 433. 50.]
[ 512. 109.]
[ 425. 109.]
[ 362. 106.]]
Object points:
[[ 0. 0. 0. ]
[ 6.5 0. 0. ]
[ 0. 0. 6.5]
[ 0. 6.5 0. ]]
Cam mat:
[[ 811.13275146 0. 322.47589111]
[ 0. 811.27490234 225.78684998]
[ 0. 0. 1. ]]
Dist Coff:
[[-0.07649349 -0.02313312 -0.00911118 -0.0049251 0.74063563]]
I am using solvePnP function with flag `SOLVEPNP_P3P`. It is giving assertion error. The same solvePnP code works fine with `SOLVEPNP_ITERATIVE` flag. With P3P flag it internally calls undistortPoints function which is giving error.
solvePnP code ref: https://github.com/opencv/opencv/blob/3.2.0/modules/calib3d/src/solvepnp.cpp
How to resolve this?djkpAThu, 18 May 2017 02:12:48 -0500http://answers.opencv.org/question/149339/Undistort image before estimating pose using solvePnPhttp://answers.opencv.org/question/98539/undistort-image-before-estimating-pose-using-solvepnp/ I have to estimate the pose of the camera using a known marker.
The camera is calibrated and I have all the calibration coefficients.
The current version of the algorithm extracts 4 co-planar points from the frame and uses them to estimate the pose using the solvePnP function.
The algorithms seems to work fine but I have a doubt. Since the solvePnP takes as input also the calibration coefficients, do I need to undistort the image before looking the 4 points?
In the following code, are the initUndistortRectifyMap/remap functions necessary?
while(1) {
frame = camera->getFrame();
imshow("frame", frame);
// Estimation of Map1 and Map2 for image rectification (to be done only on the first iteration)
if (initRectifyMap_flag)
{
// Rectify the image
// The initialization of the Rectification Parameters will be carried out only at the first frame.
initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(), getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, frame.size(), 1, frame.size(), 0), frame.size(), CV_16SC2, map1, map2);
initRectifyMap_flag = false;
}
// Remapping of the current frame
remap(frame, src, map1, map2, INTER_LINEAR, BORDER_TRANSPARENT, Scalar::all(255));
// Here is the code for the extraction of the 4 points based on the content of the variable src
...
...
// Pose estimation
solvePnP(Mat(refMarkerPoint), Mat(markerPoints), cameraMatrix, distCoeffs, rvec, tvec,false);
Alek.BMon, 18 Jul 2016 16:22:00 -0500http://answers.opencv.org/question/98539/camera pose from solvePnPRansac using 3d-2d motion estimation in monocular visual odometryhttp://answers.opencv.org/question/89197/camera-pose-from-solvepnpransac-using-3d-2d-motion-estimation-in-monocular-visual-odometry/ In paper **Visual Odometry Part I**,it said that we can get camera pose using 3d-2d motion estimation for the monocular case(It needs three images).
So, I got Rvec and tvec from function solvePnPRansac using 3d-2d motion estimation.
The question is how can I get camera position and draw its trajectory from Rvec and tvec. The following two formulas are right?
>1.Rcur=Rvec * Rcur
>2.tcur=tcur+scale*(Rcur * tvec)
Beacuase I find [Avi Singh](https://avisingh599.github.io/vision/monocular-vo/) use them in 2d-2d motion estimation.
Is there any paper about monocular visual odometry using 3d-2d motion estimation to recommend.zeb.ZThu, 03 Mar 2016 02:23:13 -0600http://answers.opencv.org/question/89197/Pose estimation using PNP: Strange wrong resultshttp://answers.opencv.org/question/76450/pose-estimation-using-pnp-strange-wrong-results/ Hello, I am trying to use the PNP algorithm implementations in Open CV (EPNP, Iterative etc.) to get the metric pose estimates of cameras in a two camera pair (not a conventional stereo rig, the cameras are free to move independent of each other). My source of images currently is a robot simulator (Gazebo), where two cameras are simulated in a scene of objects. The images are almost ideal: i.e., zero distortion, no artifacts.
So to start off, this is my first pair of images.
[![enter image description here][1]][1] [![enter image description here][2]][2]
I assume the right camera as "origin". In metric world coordinates, left camera is at (1,1,1) and right is at (-1,1,1) (2m baseline along X). Using feature matching, I construct the essential matrix and thereby the R and t of the left camera w.r.t. right. This is what I get.
R in euler angles: [-0.00462468, -0.0277675, 0.0017928]
t matrix: [-0.999999598978524; -0.0002907901840156801; -0.0008470441900959029]
Which is right, because the displacement is only along the X axis in the camera frame. For the second pair, the left camera is now at (1,1,2) (moved upwards by 1m).
[![enter image description here][3]][3] [![enter image description here][4]][4]
Now the R and t of left w.r.t. right become:
R in euler angles: [0.0311084, -0.00627169, 0.00125991]
t matrix: [-0.894611301085138; -0.4468450866008623; -0.0002975759140359637]
Which again makes sense: there is no rotation; the displacement along Y axis is half of what the baseline (along X) is, so on, although this t doesn't give me the real metric estimates.
So in order to get metric estimates of pose in case 2, I constructed the 3D points using points from camera 1 and camera 2 in case 1 (taking the known baseline into account: which is 2m), and then ran the PNP algorithm with those 3D points and the image points from case 2. Strangely, both ITERATIVE and EPNP algorithms give me a similar and completely wrong result that looks like this:
Pose according to final PNP calculation is:
Rotation euler angles: [-9.68578, 15.922, -2.9001]
Metric translation in m: [-1.944911461358863; 0.11026997013253; 0.6083336931263812]
Am I missing something basic here? I thought this should be a relatively straightforward calculation for PNP given that there's no distortion etc. ANy comments or suggestions would be very helpful, thanks!
EDIT: Code for PNP implementation
Let's say pair 1 consists of queryImg1 and trainImg1; and pair 2 consists of queryImg2 and trainImg2 (2d vectors of points). Triangulation with pair 1 results in a vector of 3D points points3D.
1. Iterate through trainImg1 and see if the same point exists in trainImg2 (because that camera does not move)
2. If the same feature is tracked in trainImg2, find the corresponding match from queryImg2.
3. Form vectors P3D_tracked (subset of tracked 3D points), P2D_tracked (subset of tracked 2d points).
for(int i = 0; i < (int)trainImg1.size(); i++)
{
vector<Point2d>::iterator iter = find(trainImg2.begin(), trainImg2.end(), trainImg1[i]);
size_t index = distance(trainImg2.begin(), iter);
if(index != trainImg2.size())
{
P3D_tracked.push_back(points3D[i]);
P2D_tracked.push_back(queryImg2[index]);
}
}
solvePnP(P3D_tracked, P2D_tracked, K, d, rvec, tvec, false, CV_EPNP);
For one example I ran, the original set of points had a size of 761, and the no. of tracked features in the second pair was 455.
[1]: http://i.stack.imgur.com/IEcPpm.jpg
[2]: http://i.stack.imgur.com/sfnlxm.jpg
[3]: http://i.stack.imgur.com/8k7xbm.jpg
[4]: http://i.stack.imgur.com/mZ7Mzm.jpgsaihvMon, 16 Nov 2015 21:27:48 -0600http://answers.opencv.org/question/76450/Is it possible to compute scale factor of subsequent stereo reconstructions if initial scale is known?http://answers.opencv.org/question/69659/is-it-possible-to-compute-scale-factor-of-subsequent-stereo-reconstructions-if-initial-scale-is-known/ I am trying to do relative pose estimation in a stereo setup using openCV, where I compute pose using the essential matrix and then try to reconstruct the scene using cv::triangulatePoints. As this gives me reconstruction up to an arbitrary scale, I thought I could calculate the subsequent scale factors if the initial baseline (~ scale) is known (and the cameras are purely translated in X). But even if the cameras do not move, and the scene does not change, if I take multiple pairs of images, each of the reconstruction is coming up with a different scale: this I know by taking two points and comparing the distances between them. The whole scene is either magnified or shrunk, the reconstruction itself is not wrong, per se.
To get around this, I tried something naive: I got the ratio of scale change between pair1 and pair2, and tried this:
1. Say distance between 3d points A and B in pair 2 = s*distance between same points in pair 1,
2. Rescale whole scene obtained from pair2 by s.
3. Use perspective n point algorithm (solvePnP in opencv) with these rescaled 3d points and 2d correspondences from pair 2.
But the answer I am getting is s*t1, where t1 is the translation in the first case. This is confusing me greatly. Because the scene has not changed by much at all, the 2d point correspondences between 1 and 2 have not changed by much at all. So is it still impossible to determine the scale of a reconstruction even though I know the initial "scale" for sure (i thought this would be straightforward)? Why is the reconstruction giving me arbitrary results at every step, and how does PnP magically know that I have done this whole rescaling? saihvFri, 28 Aug 2015 16:47:00 -0500http://answers.opencv.org/question/69659/Camera location computationhttp://answers.opencv.org/question/20396/camera-location-computation/Hi everybody,
I have a question about the way to compute the camera location. Indeed, SolvePnP gives us the rotation and translation vectors of the object in the camera space, with cv::Rodrigues we can compute the rotation matrix and build the matrix `M = [ R | T ]`.
So to have the camera location in the object space, I thought I had to compute the inverse of the matrix M, that is `M' = [ M^t | -M^t * T ]`. I did it with cv::invert but it clearly doesn't work!!
The only formula that I have seen in samples and that works is this one `M' = [ M^t | -T ]`.
Can somebody explain me why?
Many thanks.Djo1509Mon, 09 Sep 2013 15:50:30 -0500http://answers.opencv.org/question/20396/