pose_estimator.cpp
Go to the documentation of this file.
00001 #include "visual_pose_estimation/pose_estimator.h"
00002 #include "visual_pose_estimation/planar.h"
00003 
00004 namespace visual_pose_estimation {
00005 
00006 PoseEstimator::PoseEstimator()
00007   : use_planar_solve_(false)
00008 {
00009 }
00010 
00011 PoseEstimator::PoseEstimator(const cv::Mat& object_points)
00012   : object_points_(object_points),
00013     use_planar_solve_(false)
00014 {
00015 }
00016 
00017 void PoseEstimator::setObjectModel(const cv::Mat& object_points)
00018 {
00019   object_points_ = object_points;
00020 }
00021 
00022 tf::Pose PoseEstimator::solve(const std::vector<cv::Point2f>& image_points,
00023                               const image_geometry::PinholeCameraModel& model) const
00024 {
00025   tf::Pose pose;
00026   solveImpl(image_points, model, pose, false);
00027   return pose;
00028 }
00029 
00030 tf::Pose PoseEstimator::solveWithPrior(const std::vector<cv::Point2f>& image_points,
00031                                        const image_geometry::PinholeCameraModel& model,
00032                                        const tf::Pose& prior) const
00033 {
00034   tf::Pose pose = prior;
00035   solveImpl(image_points, model, pose, true);
00036   return pose;
00037 }
00038 
00039 void PoseEstimator::solveImpl(const std::vector<cv::Point2f>& image_points,
00040                               const image_geometry::PinholeCameraModel& model,
00041                               tf::Pose& pose, bool have_prior) const
00042 {
00043   // Set up buffers
00044   double R3_buffer[3], T3_buffer[3], rot3x3_buffer[9];
00045   double D_buffer[4] = {0}; // Assume image already rectified, so zero distortion.
00046   cv::Mat_<double> R3(3, 1, R3_buffer);
00047   cv::Mat_<double> T3(3, 1, T3_buffer);
00048   cv::Mat_<double> D(1, 4, D_buffer);
00049   cv::Mat_<cv::Vec2f> image_pts_cv(object_points_.rows, 1, (cv::Vec2f*)(&image_points[0]));
00050   cv::Mat_<double> rot3x3(3, 3, rot3x3_buffer);
00051 
00052   if (have_prior) {
00053     T3(0,0) = pose.getOrigin().x();
00054     T3(1,0) = pose.getOrigin().y();
00055     T3(2,0) = pose.getOrigin().z();
00056     
00057     // Convert to Rodrigues rotation
00058     tf::Matrix3x3 &basis = pose.getBasis();
00059     for (int i = 0; i < 3; ++i)
00060       for (int j = 0; j < 3; ++j)
00061         rot3x3(i,j) = basis[i][j];
00062     cv::Rodrigues(rot3x3, R3);
00063     ROS_DEBUG("Prior pose: T(%.3f, %.3f, %.3f), R(%.3f, %.3f, %.3f)",
00064               T3(0,0), T3(1,0), T3(2,0), R3(0,0), R3(1,0), R3(2,0));
00065   }
00066 
00067   // First three columns of projection matrix
00068   cv::Mat_<double> projection(3,3);
00069   for (int i = 0; i < 3; ++i) {
00070     for (int j = 0; j < 3; ++j) {
00071       projection(i,j) = model.projectionMatrix()(i,j);
00072     }
00073   }
00074 
00075   // Find/refine object pose
00076   if (use_planar_solve_)
00077     cv::solvePlanarPnP(object_points_, image_pts_cv, projection, D, R3, T3, have_prior);
00078   else
00079     cv::solvePnP(object_points_, image_pts_cv, projection, D, R3, T3, have_prior);
00080   
00081   ROS_DEBUG("Refined pose: T(%.3f, %.3f, %.3f), R(%.3f, %.3f, %.3f)",
00082             T3(0,0), T3(1,0), T3(2,0), R3(0,0), R3(1,0), R3(2,0));
00083   pose.getOrigin().setValue(T3(0,0), T3(1,0), T3(2,0));
00084 
00085   // Convert from Rodrigues to rotation matrix
00086   cv::Rodrigues(R3, rot3x3);
00087   tf::Matrix3x3 rot3x3_tf(rot3x3(0,0), rot3x3(0,1), rot3x3(0,2),
00088                           rot3x3(1,0), rot3x3(1,1), rot3x3(1,2),
00089                           rot3x3(2,0), rot3x3(2,1), rot3x3(2,2));
00090   pose.setBasis(rot3x3_tf);
00091 }
00092 
00093 } //namespace visual_pose_estimation


visual_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Thu Aug 27 2015 14:29:48