I have a fish-eye camera in the ceiling and I want to locate some points on the floor. I have put the origin of my reference system (real world) just below the camera and I want to know the position of every object in centimeters. This picture shows this:
Reference system - Real world
Firstly, I have done the camera calibration and I have obtained the next result with an RMS of 1.11:
Undistorted image after calibration
As a result of the calibration I obtained intrinsic parameters (camera matrix), so I used cv::solvePnP to get rotation and translation vectors. For apply this I marked some points in the undistorted image (in pixels) and I measured them in real world according to my reference system.
For example, the origin is in the center of a 1024x768 image, so:
Point 0: ImagePoint(512, 384) [pixels] --> ObjectPoint(0,0) [centimeters]
The next code shows this:
std::vector<cv::Point2f> imagePointsPix;
std::vector<cv::Point3f> objectPointsCm;
imagePointsPix.push_back(cv::Point2f(512.,384.));
imagePointsPix.push_back(cv::Point2f(404.,512.));
imagePointsPix.push_back(cv::Point2f(666.,211.));
imagePointsPix.push_back(cv::Point2f(519.,66.));
objectPointsCm.push_back(cv::Point3f(0., 0., 0.));
objectPointsCm.push_back(cv::Point3f(-80.,-132.,0.));
objectPointsCm.push_back(cv::Point3f(120.,188.,0.));
objectPointsCm.push_back(cv::Point3f(-40.,268.,0.));
cv::Mat rvec(1,3,cv::DataType<double>::type);
cv::Mat tvec(1,3,cv::DataType<double>::type);
cv::Mat rotationMatrix(3,3,cv::DataType<double>::type);
cv::solvePnP(objectPointsCm, imagePointsPix, cameraMatrix, distCoeffs, rvec, tvec, 0, SOLVEPNP_ITERATIVE);
cv::Rodrigues(rvec,rotationMatrix);
Now I have the camera matrix, the rotation matrix and the traslation vector, so by using this as reference I am able to compute any point if I have its position in pixels. This is the code:
cv::Mat uvPoint = cv::Mat::ones(3,1,cv::DataType<double>::type); //u,v,1
uvPoint.at<double>(0,0) = 512.; //img point for which we want its real coordinates
uvPoint.at<double>(1,0) = 384.;
cv::Mat tempMat, tempMat2;
double s;
tempMat = rotationMatrix.inv() * cameraMatrix.inv() * uvPoint;
tempMat2 = rotationMatrix.inv() * tvec;
s = 0 + tempMat2.at<double>(2,0); //before 0 it was 285, which represents the height Zconst
s /= tempMat.at<double>(2,0);
std::cout << "P = " << rotationMatrix.inv() * (s * cameraMatrix.inv() * uvPoint - tvec) << std::endl;
I get this results for the same points I used for obtaining my parameters:
Point 0 --> (0.213, 3.391) (it should be (0,0)) ERROR: 3.69 cm
Point 1 --> (-68.28, -112.82) (it should be (-80, -132)) ERROR: 17.49 cm
Point 2 --> (84.48, 137.61) (it should be (120, 188)) ERROR: 49.62 cm
The rest of points also show an error too big... I have used more points but the results do not improve. I don't know where I went wrong, could anyone help me?
Thanks in advance.
It looks like you may be effectively undistorting your image twice from solvePNP's perspective. This is due to passing in the distortion coefficients along with point correspondences that are already derived from an undistorted image.
Try passing the actual camera matrix from your calibration to solvePNP instead of an identity matrix, but still pass NULL for the distortion coefficients to avoid the double-undistortion.
Finally I have found out that the error was caused by the distortion coefficients, i.e. my calibration. I set the cameraMatrix to the Identity matrix (eye(3)) and the distCoefficients to NULL so that solvePNP assumed I have a perfect camera. Using this approach I obtained an error much lower. I will have to make a better calibration.
Related
I am trying to perform 3D Reconstruction(Structure From Motion) from Multiple Images of Planar Markers. I very new to MVG and openCV.
As far I have understood I have to do the following steps:
Identify corresponding 2D corner points in the one images.
Calculate the Camera Pose of the first image us cv::solvePNP(assuming the
origin to be center of the marker).
Repeat 1 and 2 for the second image.
Estimate the relative motion of the camera by Rot_relative = R2 - R1,
Trans_relative = T2-T1.
Now assume the first camera to be the origin construct the 3x4 Projection
Matrix for both views, P1 =[I|0]*CameraMatrix(known by Calibration) and P2 =
[Rot_relative |Trans_relative ].
Use the created projection matrices and 2D corner points to triangulate the
3D coordinate using cv::triangulatePoints(P1,P2,point1,point2,OutMat)
The 3D coordinate can be found by dividing the each rows of OutMat by the 4th
row.
I was hoping to keep my "First View" as my origin and iterate
through n views repeating steps from 1-7(I suppose its called Global SFM).
I was hoping to get (n-1)3D points of the corners with "The first View as origin" which we could optimize using Bundle Adjustment.
But the result I get is very disappointing the 3D points calculated are displaced by a huge factor.
These are questions:
1.Is there something wrong with the steps I followed?
2.Should I use cv::findHomography() and cv::decomposeHomographyMat() to find the
relative motion of the camera?
3.Should point1 and point2 in cv::triangulatePoints(P1,P2,point1,point2,OutMat)
be normalized and undistorted? If yes, how should the "Outmat" be interpreted?
Please anyone who has insights towards the topic, can you point out my mistake?
P.S. I have come to above understanding after reading "MultiView Geometry in Computer Vision"
Please find the code snippet below:
cv::Mat Reconstruction::Triangulate(std::vector<cv::Point2f>
ImagePointsFirstView, std::vector<cv::Point2f>ImagePointsSecondView)
{
cv::Mat rVectFirstView, tVecFristView;
cv::Mat rVectSecondView, tVecSecondView;
cv::Mat RotMatFirstView = cv::Mat(3, 3, CV_64F);
cv::Mat RotMatSecondView = cv::Mat(3, 3, CV_64F);
cv::solvePnP(RealWorldPoints, ImagePointsFirstView, cameraMatrix, distortionMatrix, rVectFirstView, tVecFristView);
cv::solvePnP(RealWorldPoints, ImagePointsSecondView, cameraMatrix, distortionMatrix, rVectSecondView, tVecSecondView);
cv::Rodrigues(rVectFirstView, RotMatFirstView);
cv::Rodrigues(rVectSecondView, RotMatSecondView);
cv::Mat RelativeRot = RotMatFirstView-RotMatSecondView ;
cv::Mat RelativeTrans = tVecFristView-tVecSecondView ;
cv::Mat RelativePose;
cv::hconcat(RelativeRot, RelativeTrans, RelativePose);
cv::Mat ProjectionMatrix_0 = cameraMatrix*cv::Mat::eye(3, 4, CV_64F);
cv::Mat ProjectionMatrix_1 = cameraMatrix* RelativePose;
cv::Mat X;
cv::undistortPoints(ImagePointsFirstView, ImagePointsFirstView, cameraMatrix, distortionMatrix, cameraMatrix);
cv::undistortPoints(ImagePointsSecondView, ImagePointsSecondView, cameraMatrix, distortionMatrix, cameraMatrix);
cv::triangulatePoints(ProjectionMatrix_0, ProjectionMatrix_1, ImagePointsFirstView, ImagePointsSecondView, X);
X.row(0) = X.row(0) / X.row(3);
X.row(1) = X.row(1) / X.row(3);
X.row(2) = X.row(2) / X.row(3);
return X;
}
I am trying to measure the pose of a camera and I have done the following.
Mark world 3-D(Assuming z=0, since it is flat) points on corners of a square on a flat surface and assume a world coordinate system.(in cms)
Have taken the top left corner of the square as my origin and given the world points in the following order(x,y)or(col,row):
(0,0),(12.8,0),(12.8,12.8),(0,12.8) - in cms
Detect those points in my image.(in pixels)
The image points and world points are in the same order.
I have calibrated my camera for intrinsic matrix and distortion coefficients.
I use SolvePnP function to get rvec and tvec.
I use Rodrigues function to get rotation matrix.
To check if rvec and tvec is correct, I project back the 3-D points(z=0) using ProjectPoints into the image plane and I get the points correctly on my image with an error of 3 pixels on X- axis.
Now I go ahead and calculate my camera position in the world frame using the formula:
cam_worl_pos = - inverse(R) * tvec. (This formula I have verified in many blogs and also this makes sense)
But my cam_worl_pos x,y, and z in cms do not seem to be correct.
My doubt is, if I am able to project back the 3-D world point to image plane using rvec and tvec with (3 pixel error on X-axis and almost no error on Y axis, hope it is not too bad), then why am I not getting the camera position in world frame right.
Also, I have a doubt on SolvPnP rvec and tvec solution, they might be one of the multiple solutions, but not the one which I want.
How do I get the right rvec and tvec from SolvPnp or any other suggestions to get rvec and tvec would also be helpful.
EDITS :
Image Size - 720(row) * 1280(col)
camera parameters
cameraMatrix_Front=[908.65 0 642.88
0 909.28 364.95
0 0 1]
distCoeffs_Front=[-0.4589, 0.09462, -1.46*10^-3, 1.23*10^-3]
OpenCV C++ code:
vector<Point3f> front_object_pts;
Mat rvec_front;
Mat tvec_front;
Mat rotation_front;
Mat world_position_front_cam;
//Fill front object points(x-y-z order in cms)
//It is square of side 12.8cms on Z=0 plane
front_object_pts.push_back(Point3f(0, 0, 0));
front_object_pts.push_back(Point3f(12.8, 0, 0));
front_object_pts.push_back(Point3f(12.8,12.8,0));
front_object_pts.push_back(Point3f(0, 12.8, 0));
//Corresponding Image points detected in the same order as object points
front_image_pts.push_back(points_front[0]);
front_image_pts.push_back(points_front[1]);
front_image_pts.push_back(points_front[2]);
front_image_pts.push_back(points_front[3]);
//Detected points in image matching the 3-D points in the same order
//(467,368)
//(512,369)
//(456,417)
//(391,416)
//Get rvec and tvec using Solve PnP
solvePnP(front_object_pts, front_image_pts, cameraMatrix_Front,
Mat(4,1,CV_64FC1,Scalar(0)), rvec_front, tvec_front, false, CV_ITERATIVE);
//Output of SolvePnP
//tvec=[-26.951,0.6041,134.72] (3 x 1 matrix)
//rvec=[-1.0053,0.6691,0.3752] (3 x 1 matrix)
//Check rvec and tvec is correct or not by projecting the 3-D object points to image
vector<Point2f>check_front_image_pts
projectPoints(front_object_pts, rvec_front, tvec_front,
cameraMatrix_Front, distCoeffs_Front, check_front_image_pts);
//Here to note that I have made **distCoefficents**,
//a 0 vector since my image points are detected after radial distortion is removed
//Get rotation matrix
Rodrigues(rvec_front, rotation_front);
//Get rotation matrix inverse
Mat rotation_inverse;
transpose(rotation_front, rotation_inverse);
//Get camera position in world cordinates
world_position_front_cam = -rotation_inverse * tvec_front;
//Actual location of camera(Measured manually)
X=47cm
Y=18cm
Z=25cm
//Obtained location
X=110cm
Y=71cm
Z=-40cm
I succesfully calibrate my camera using opencv. The camera lens i am using.
https://www.baslerweb.com/en/products/vision-components/lenses/basler-lens-c125-0418-5m-f1-8-f4mm/
The internal and external camera parameter is given below.
cv::Mat cameraMatrix(3, 3, cv::DataType<double>::type);
cameraMatrix.at<double>(0) = 1782.80;//fx //432.2 in mm
cameraMatrix.at<double>(1) = 0;
cameraMatrix.at<double>(2) = 3.0587694283633488e+002;//cx
cameraMatrix.at<double>(3) = 0;
cameraMatrix.at<double>(4) = 1782.80;//fy
cameraMatrix.at<double>(5) = 3.0535864258476721e+002;//cy
cameraMatrix.at<double>(6) = 0;
cameraMatrix.at<double>(7) = 0;
cameraMatrix.at<double>(8) = 1;
cv::Mat disCoeffs(1, 5, cv::DataType<double>::type);
disCoeffs.at<double>(0) = -8.1752937039996709e-001;//k1
disCoeffs.at<double>(1) = -2.5660653367749450e+001;//k2
disCoeffs.at<double>(2) = -1.5556922931812768e-002;//p1
disCoeffs.at<double>(3) = -4.4021541217208054e-002;//p2
disCoeffs.at<double>(4) = 1.5042036073609015e+002;//k3
I know this formula is used to calculate the distance of the object. But i am very confuse how to proper use it.
Resolution of my camera is 640x480.
focal length = 1782.80 (px) do not know how to correctly convert to mm
i know focal length is distance from sensor to image plane. So what actually this value represent? Pixel is just a unit represent dot on screen.
Object i am using is circle.
radius = 22. (width and height 44*44)
circle center point: 300,300 (x,y)
sensor height do not know how to get?
Where do i use principle points?
How i get distance from camera to object? How do get real world coordinate of the circle?
I know its too much to ask. I try one month. Did not find any proper solution.
i use function solvePnP to get the camera translation and rotation matrix. But i have problem how to calculate object point?
Your cx and cy seems to be wrong because they should be half the resolution: 640/2 & 480/2.
fx and fy are in pixel unit you get from calibration process. To convert them to mm use that formula:
pixels width = (image width in pixels) * (focal length in mm) / (CCD width in mm)
pixels height = (image height in pixels) * (focal length in mm) / (CCD height in mm)
When you calibrate your camera, you use those formulas to make sure you've the right values. For me cx and cy are wrong because they represent the center of the image (they shouldn't be equal unless your image is square which is not the case). For fx and fy I can't tell because I don't know the CCD of your camera. They can be equal if the CCD is square.
Don't change those parameters manually but let the your calibration software compute them.
Now you've those parameters, how you compute the distance?
The formula you presented is not useful in a sense that if you can measure the real height, you usually can measure the distance (at least in your case).. so why using a camera!?
So to compute the distance in real world, you need two more things: The extrinsic parameters (Your cameraMatrix matrix is the intrinsic parameters) and at least four points (the more points the better) in real world coordinates.
Once you have those things, you can use solvePnP function to find the pose of an object. The pose represents the translation and rotation with respect to the camera frame.
http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#solvepnp
This is a piece of code can help to do that:
//Four points in real world with `x,y and z` coordinates
vector<Point3f> vec3d;
vec3d.push_back(Point3f(0, 0, 0));
vec3d.push_back(Point3f(0, 211, 0));
vec3d.push_back(Point3f(295, 211, 0));
vec3d.push_back(Point3f(295, 0, 0));
The z=0 because your real points are in a plane.
//The same four points but in your image plan, therefore there is no z and they're in pixel unit
vector<Point2f> vec2d;
vec2d.push_back(Point2f(532, 412)); //(y,x)
vec2d.push_back(Point2f(583, 594));
vec2d.push_back(Point2f(927, 535));
vec2d.push_back(Point2f(817, 364));
//The pose of the object: rvec is your rotation vector, tvec is your translation vector
cv::Mat rvec, tvec;
solvePnP(vec3d, vec2d, cameraMatrix, distCoeffs, rvec, tvec);
Finally, you can compute the real distance from the tvec as euclidean distance: d=std::sqrt(tx*tx+ty*ty+tz*tz).
Your questions:
sensor height do not know how to get?
Look for your camera specification in the internet or in the manual book and you'll find it.
Where do i use principle points?
They're your intrinsic parameters. You're not gonna use them separately.
How i get distance from camera to object? How do get real world coordinate of the circle?
I explained that above. You need four points and with a circle you have only one which not enough to compute the pose.
But i have problem how to calculate object point?
objectPoints in solvePnP are your real world coordinates. For example, a chessboard has corners in which we know the exact position in mm of each one with respect to a world frame that you choose in the chessboard. It can be in the left top corner or something like that and z=0 because the chessboard is printed in a paper just like your circle!
EDIT:
You can find more specifications in the manual page 13 here. It is said 7.4 x 7.4µm:
f (mm)=f(pixel) x pixel_size(mm) => f (mm) = 1782.80x7.2e-6 = 12.83616 (mm)
Which is not 4mm!! then you need to do the calibration again, something is wrong!
3D points:
vector vec3d;
vec3d is where you gonna store your 3D coordinates point. I gave you an example for the first point which the origin:
vec3d.push_back(Point3f(0, 0, 0)); //y,x,z
EDIT3
If you take a pattern like this
Then choose for example the circle in top left or right corner and it will have a coordinate of (0,0,0), that the origin. After that the circle next to it is your second point and it will have (x,0,0) x is the distance in (mm) between the two circles.. You do the same for four points in your pattern. You can choose any pattern you want as long as you can detect it in your image and retrieve their coordinates in pixel.
If you still don't understand, I advise you take a course in projective geometry and camera models.. so as you can understand what every parameter means.
I'm to build a panorama image of the ground covered by a downward facing camera (at a fixed height, around 1 metre above ground). This could potentially run to thousands of frames, so the Stitcher class' built in panorama method isn't really suitable - it's far too slow and memory hungry.
Instead I'm assuming the floor and motion is planar (not unreasonable here) and trying to build up a cumulative homography as I see each frame. That is, for each frame, I calculate the homography from the previous one to the new one. I then get the cumulative homography by multiplying that with the product of all previous homographies.
Let's say I get H01 between frames 0 and 1, then H12 between frames 1 and 2. To get the transformation to place frame 2 onto the mosaic, I need to get H01*H12. This continues as the frame count increases, such that I get H01*H12*H23*H34*H45*....
In code, this is something akin to:
cv::Mat previous, current;
// Init cumulative homography
cv::Mat cumulative_homography = cv::Mat::eye(3);
video_stream >> previous;
for(;;) {
video_stream >> current;
// Here I do some checking of the frame, etc
// Get the homography using my DenseMosaic class (using Farneback to get OF)
cv::Mat tmp_H = DenseMosaic::get_homography(previous,current);
// Now normalise the homography by its bottom right corner
tmp_H /= tmp_H.at<double>(2, 2);
cumulative_homography *= tmp_H;
previous = current.clone( );
}
It works pretty well, except that as the camera moves "up" in the viewpoint, the homography scale decreases. As it moves down, the scale increases again. This gives my panoramas a perspective type effect that I really don't want.
For example, this is taken on a few seconds of video moving forward then backward. The first frame looks ok:
The problem comes as we move forward a few frames:
Then when we come back again, you can see the frame gets bigger again:
I'm at a loss as to where this is coming from.
I'm using Farneback dense optical flow to calculate pixel-pixel correspondences as below (sparse feature matching doesn't work well on this data) and I've checked my flow vectors - they're generally very good, so it's not a tracking problem. I also tried switching the order of the inputs to find homography (in case I'd mixed up the frame numbers), still no better.
cv::calcOpticalFlowFarneback(grey_1, grey_2, flow_mat, 0.5, 6,50, 5, 7, 1.5, flags);
// Using the flow_mat optical flow map, populate grid point correspondences between images
std::vector<cv::Point2f> points_1, points_2;
median_motion = DenseMosaic::dense_flow_to_corresp(flow_mat, points_1, points_2);
cv::Mat H = cv::findHomography(cv::Mat(points_2), cv::Mat(points_1), CV_RANSAC, 1);
Another thing I thought it could be was the translation I include in the transformation to ensure my panorama is centred within the scene:
cv::warpPerspective(init.clone(), warped, translation*homography, init.size());
But having checked the values in the homography before the translation is applied, the scaling issue I mention is still present.
Any hints are gratefully received. There's a lot of code I could put in but it seems irrelevant, please do let me know if there's something missing
UPDATE
I've tried switching out the *= operator for the full multiplication and tried reversing the order the homographies are multiplied in, but no luck. Below is my code for calculating the homography:
/**
\brief Calculates the homography between the current and previous frames
*/
cv::Mat DenseMosaic::get_homography()
{
cv::Mat grey_1, grey_2; // Grayscale versions of frames
cv::cvtColor(prev, grey_1, CV_BGR2GRAY);
cv::cvtColor(cur, grey_2, CV_BGR2GRAY);
// Calculate the dense flow
int flags = cv::OPTFLOW_FARNEBACK_GAUSSIAN;
if (frame_number > 2) {
flags = flags | cv::OPTFLOW_USE_INITIAL_FLOW;
}
cv::calcOpticalFlowFarneback(grey_1, grey_2, flow_mat, 0.5, 6,50, 5, 7, 1.5, flags);
// Convert the flow map to point correspondences
std::vector<cv::Point2f> points_1, points_2;
median_motion = DenseMosaic::dense_flow_to_corresp(flow_mat, points_1, points_2);
// Use the correspondences to get the homography
cv::Mat H = cv::findHomography(cv::Mat(points_2), cv::Mat(points_1), CV_RANSAC, 1);
return H;
}
And this is the function I use to find the correspondences from the flow map:
/**
\brief Calculate pixel->pixel correspondences given a map of the optical flow across the image
\param[in] flow_mat Map of the optical flow across the image
\param[out] points_1 The set of points from #cur
\param[out] points_2 The set of points from #prev
\param[in] step_size The size of spaces between the grid lines
\return The median motion as a point
Uses a dense flow map (such as that created by cv::calcOpticalFlowFarneback) to obtain a set of point correspondences across a grid.
*/
cv::Point2f DenseMosaic::dense_flow_to_corresp(const cv::Mat &flow_mat, std::vector<cv::Point2f> &points_1, std::vector<cv::Point2f> &points_2, int step_size)
{
std::vector<double> tx, ty;
for (int y = 0; y < flow_mat.rows; y += step_size) {
for (int x = 0; x < flow_mat.cols; x += step_size) {
/* Flow is basically the delta between left and right points */
cv::Point2f flow = flow_mat.at<cv::Point2f>(y, x);
tx.push_back(flow.x);
ty.push_back(flow.y);
/* There's no need to calculate for every single point,
if there's not much change, just ignore it
*/
if (fabs(flow.x) < 0.1 && fabs(flow.y) < 0.1)
continue;
points_1.push_back(cv::Point2f(x, y));
points_2.push_back(cv::Point2f(x + flow.x, y + flow.y));
}
}
// I know this should be median, not mean, but it's only used for plotting the
// general motion direction so it's unimportant.
cv::Point2f t_median;
cv::Scalar mtx = cv::mean(tx);
t_median.x = mtx[0];
cv::Scalar mty = cv::mean(ty);
t_median.y = mty[0];
return t_median;
}
It turns out this was because my viewpoint was close to the features, meaning that the non-planarity of the tracked features was causing skew to the homography. I managed to prevent this (it's more of a hack than a method...) by using estimateRigidTransform instead of findHomography, as this does not estimate for perspective variations.
In this particular case, it makes sense to do so, as the view does only ever undergo rigid transformations.
I am having a lot of trouble applying RVEC and TVEC (the estimated camera pose) from OpenCV's cv::solvePnP to a vtkCamera that I have in a virtual 3D scene. I am hoping someone can show me the errors I am making.
I am trying to take this vtkActor (3D DICOM Rendering of my chest with fiduciary markers placed on my torso):
and use cv::solvePnP to align the fiduciary markers with the red circles shown in the following image (note: the red circles are hard-coded coordinates from a picture of the fiduciary markers with a certain camera perspective):
As you can see, the super-imposed volume rendering is mis-aligned after applying the following vtkTransform to the vtkCamera.
cv::Mat op(model_points);
cv::Mat rvec;
cv::Mat tvec;
// op = the 3D coordinates of the markers in the scene
// ip = the 2D coordinates of the markers in the image
// camera = the intrinsic camera parameters (for calibration)
// dists = the camera distortion coefficients
cv::solvePnP(op, *ip, camera, dists, rvec, tvec, false, CV_ITERATIVE);
cv::Mat rotM;
cv::Rodrigues(rvec, rotM);
rotM = rotM.t();
cv::Mat rtvec = -(rotM*tvec);
std::cout << "rotM: \n" << rotM << std::endl;
std::cout << "tvec: \n" << tvec << std::endl;
std::cout << "rtvec: \n" << rtvec << std::endl;
double cam[16] = {
rotM.at<double>(0), rotM.at<double>(1), rotM.at<double>(2), rtvec.at<double>(0),
rotM.at<double>(3), rotM.at<double>(4), rotM.at<double>(5), rtvec.at<double>(1),
rotM.at<double>(6), rotM.at<double>(7), rotM.at<double>(8), rtvec.at<double>(2),
0, 0, 0, 1
};
vtkSmartPointer<vtkTransform> T = vtkSmartPointer<vtkTransform>::New();
T->SetMatrix(cam);
vtkSmartPointer<vtkRenderer> renderer = v->renderer();
double b_p[3];
double a_p[3];
double *b_o;
double b_o_store[3];
double *a_o;
double b_f[3];
double a_f[3];
vtkSmartPointer<vtkCamera> scene_camera = v->camera();
// Reset Position/Focal/Orientation before applying transformation
// so the transform does not compound
v->ResetCameraPositionOrientation();
// Apply the transformation
scene_camera->ApplyTransform(T);
scene_camera->SetClippingRange(1, 2000);
This is emphasized in the following capture of the scene (the chest is bowed, towards the screen, you can see three of the top fiduciary markers at the bottom-most part of the actor in the scene):
The following screenshot shows the RVEC & TVEC I get, as well as the Position/Orientation/Focal-Point before and after the transformation:
The scene is initialized in the following manner:
this->actor_ = vtkVolume::New();
this->actor_->SetMapper(mapper);
this->actor_->SetProperty(volumeProperty);
this->actor_->SetPosition(0,0,0);
this->actor_->RotateX(90.0);
this->renderer_ = vtkRenderer::New();
this->renderer_->AddViewProp(this->actor_);
this->renderer_->SetBackground(0.3,0.3,0.3);
this->camera_ = this->renderer_->GetActiveCamera();
// Center the scene so that we can grab the position/focal-point for later
// use.
this->renderer_->ResetCamera();
// Get the position/focal-point for later use.
double pos[3];
double orientation[3];
this->camera_->GetPosition(pos);
this->camera_->GetFocalPoint(this->focal_);
double *_o = this->camera_->GetOrientation();
this->orientation_[0] = _o[0];
this->orientation_[1] = _o[1];
this->orientation_[2] = _o[2];
this->position_[0] = pos[0];
this->position_[1] = pos[1];
this->position_[2] = pos[2];
// Set the camera in hopes of it "sticking"
this->camera_->SetPosition(pos);
this->camera_->SetFocalPoint(this->focal_);
this->camera_->SetViewUp(0, 1, 0);
this->camera_->SetFreezeFocalPoint(true);
I apologize for such a long question. I wanted to provide as much information as possible. I have been working on this problem for a few days now and cannot figure it out!
this may be too late but I'm doing nearly the exact thing currently and we just addressed this issue. I'm even using VTK and everything! What SolvePnP returns (Assuming that I am not mistaken that OpenCV's Rodrigues returns a similar matrix) is a global transformation matrix. This matrix represents rotations and translations in the global frame. Due to the way transformations work, globals must be premultiplied whereas local transformations are postmultiplied, so that needs to be done in your code. The way we did this was to use the line
((vtkMRMLLinearTransformNode*)(d->CameraVideoTransformComboBox->currentNode()))->SetMatrixTransformFromParent(staticVideoTransform4x4);
where:
d is a reference to the UI of a 3d Slicer module, which is a program that can basically be treated like a big VTK toolbox
CameraVideoTransformComboBox is just a UI combo box that stores transforms
and staticVideoTransform4x4 is our translational matrix.
We then applied the transform via the UI as opposed to your method of doing it through code, so unfortunately there's some black boxing in there that doesn't let me give you an exact answer as to how you'd have to code it yourself. I would recommend looking at vtkMRMLTransformNode::SetMatrixTransformFromParent() for guidance if you (or more likely someone reading this) is having this issue. If that doesn't work outright, try inverting the matrix!
You can invert matrix returned from solvePnP() and use cv::viz::Viz3d::setViewerPose() sources as example