From 5ecbf59dd116658a6fc039d33a0270b4d4b317b5 Mon Sep 17 00:00:00 2001
From: Dirk Thomas <dirk-thomas@users.noreply.github.com>
Date: Thu, 11 May 2017 16:19:05 -0700
Subject: [PATCH] avoid passing std::vector to/from cv::projectPoints

---
 image_geometry/src/pinhole_camera_model.cpp | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/image_geometry/src/pinhole_camera_model.cpp b/image_geometry/src/pinhole_camera_model.cpp
index b3c7376a8..0bfdebc91 100644
--- a/image_geometry/src/pinhole_camera_model.cpp
+++ b/image_geometry/src/pinhole_camera_model.cpp
@@ -376,9 +376,18 @@ cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
   cv::Mat r_vec, t_vec = cv::Mat_<double>::zeros(3, 1);
   cv::Rodrigues(R_.t(), r_vec);
   std::vector<cv::Point2d> image_point;
-  cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K_, D_, image_point);
 
-  return image_point[0];
+  // passing std::vector<..> to / from cv::projectPoints
+  // seems to not work on Windows in Debug mode with VS 2015
+  cv::Mat mat_in(1, 3, CV_64F);
+  mat_in.at<double>(0, 0) = ray.x;
+  mat_in.at<double>(0, 1) = ray.y;
+  mat_in.at<double>(0, 2) = ray.z;
+  cv::Mat mat_out(1, 1, CV_64F);
+
+  cv::projectPoints(mat_in, r_vec, t_vec, K_, D_, mat_out);
+
+  return cv::Point2d(mat_out.at<double>(0, 0), mat_out.at<double>(0, 1));
 }
 
 cv::Rect PinholeCameraModel::rectifyRoi(const cv::Rect& roi_raw) const