VTK camera pose from OpenCV's estimated pose from solvePnP - c++

Question

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!

Answer 1

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!

Answer 2

You can invert matrix returned from solvePnP() and use cv::viz::Viz3d::setViewerPose() sources as example