OpenCV/WishList/ProjectPoints

This example has a projection matrix and corresponding 3D/2D points hard coded. The idea is to project the 3D points using the projection matrix, and demonstrate that the 3D points get projected to near their corresponding 2D point.

OpenCV Error: Formats of input arguments do not match (All the matrices must have the same data type) in cvRodrigues2, file /home/ddoria/src/OpenCV- 2.3.0/modules/calib3d/src/calibration.cpp, line 507
terminate called after throwing an instance of 'cv::Exception'
 what():  /home/ddoria/src/OpenCV-2.3.0/modules/calib3d/src/calibration.cpp:507: error: (-205) All the matrices must have the same data type in function cvRodrigues2

ProjectPoints.cxx

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
 
#include <iostream>
#include <string>
 
std::vector<cv::Point2f> Generate2DPoints();
std::vector<cv::Point3f> Generate3DPoints();
 
int main( int argc, char* argv[])
{
  // Read points
  std::vector<cv::Point2f> imagePoints = Generate2DPoints();
  std::vector<cv::Point3f> objectPoints = Generate3DPoints();
 
  // Create the known projection matrix
  cv::Mat P(3,4,cv::DataType<float>::type);
  P.at<float>(0,0) = -2.8058e-01;
  P.at<float>(1,0) = -6.8326e-02;
  P.at<float>(2,0) = 5.1458e-07;
 
  P.at<float>(0,1) = 2.0045e-02;
  P.at<float>(1,1) = -3.1718e-01;
  P.at<float>(2,1) = 4.5840e-06;
 
  P.at<float>(0,2) = 1.8102e-01;
  P.at<float>(1,2) = -7.2974e-02;
  P.at<float>(2,2) = 2.6699e-06;
 
  P.at<float>(0,3) = 6.6062e-01;
  P.at<float>(1,3) = 5.8402e-01;
  P.at<float>(2,3) = 1.5590e-03;
 
    // Decompose the projection matrix into:
  cv::Mat K(3,3,cv::DataType<float>::type); // intrinsic parameter matrix
  cv::Mat rvec(3,3,cv::DataType<float>::type); // rotation matrix
  cv::Mat T(4,1,cv::DataType<float>::type); // translation vector
 
  cv::decomposeProjectionMatrix(P, K, rvec, T);
 
  // Create zero distortion
  cv::Mat distCoeffs(4,1,cv::DataType<float>::type);
  distCoeffs.at<float>(0) = 0;
  distCoeffs.at<float>(1) = 0;
  distCoeffs.at<float>(2) = 0;
  distCoeffs.at<float>(3) = 0;
 
  std::vector<cv::Point2f> projectedPoints;
  cv::projectPoints(objectPoints, rvec, T, K, distCoeffs, projectedPoints);
 
  for(unsigned int i = 0; i < projectedPoints.size(); ++i)
    {
    std::cout << "Image point: " << imagePoints[i] << " Projected to " << projectedPoints[i] << std::endl;
    }
 
  return 0;
}
 
 
std::vector<cv::Point2f> Generate2DPoints()
{
  std::vector<cv::Point2f> points;
 
  float x,y;
 
  x=282;y=274;
  points.push_back(cv::Point2f(x,y));
 
  x=397;y=227;
  points.push_back(cv::Point2f(x,y));
 
  x=577;y=271;
  points.push_back(cv::Point2f(x,y));
 
  x=462;y=318;
  points.push_back(cv::Point2f(x,y));
 
  x=270;y=479;
  points.push_back(cv::Point2f(x,y));
 
  x=450;y=523;
  points.push_back(cv::Point2f(x,y));
 
  x=566;y=475;
  points.push_back(cv::Point2f(x,y));
  /*
  for(unsigned int i = 0; i < points.size(); ++i)
    {
    std::cout << points[i] << std::endl;
    }
  */
  return points;
}
 
 
std::vector<cv::Point3f> Generate3DPoints()
{
  std::vector<cv::Point3f> points;
 
  float x,y,z;
 
  x=.5;y=.5;z=-.5;
  points.push_back(cv::Point3f(x,y,z));
 
  x=.5;y=.5;z=.5;
  points.push_back(cv::Point3f(x,y,z));
 
  x=-.5;y=.5;z=.5;
  points.push_back(cv::Point3f(x,y,z));
 
  x=-.5;y=.5;z=-.5;
  points.push_back(cv::Point3f(x,y,z));
 
  x=.5;y=-.5;z=-.5;
  points.push_back(cv::Point3f(x,y,z));
 
  x=-.5;y=-.5;z=-.5;
  points.push_back(cv::Point3f(x,y,z));
 
  x=-.5;y=-.5;z=.5;
  points.push_back(cv::Point3f(x,y,z));
 
  /*
  for(unsigned int i = 0; i < points.size(); ++i)
    {
    std::cout << points[i] << std::endl;
    }
  */
  return points;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.6)
 
PROJECT(ProjectPoints)
 
FIND_PACKAGE(OpenCV REQUIRED )
INCLUDE_DIRECTORIES( ${OPENCV_INCLUDE_DIR} )
 
ADD_EXECUTABLE(ProjectPoints ProjectPoints.cxx)
TARGET_LINK_LIBRARIES(ProjectPoints opencv_core opencv_highgui
opencv_flann opencv_imgproc opencv_highgui opencv_ml opencv_video opencv_objdetect
          opencv_features2d opencv_calib3d opencv_legacy opencv_contrib
)