예제 #1
0
PoseRT PoseRT::operator*(const PoseRT &pose) const
{
  PoseRT result;
  composeRT(pose.getRvec(), pose.getTvec(), rvec, tvec, result.rvec, result.tvec);
  //composeRT(rvec, tvec, pose.getRvec(), pose.getTvec(), result.rvec, result.tvec);
  return result;
}
void publishPoints(const std::vector<cv::Point3f>& points, const boost::shared_ptr<pcl::visualization::PCLVisualizer> &viewer, cv::Scalar color, const std::string &title, const PoseRT &pose)
{
  vector<Point3f> rotatedPoints;
  project3dPoints(points, pose.getRvec(), pose.getTvec(), rotatedPoints);
  pcl::PointCloud<pcl::PointXYZ> pclPoints;
  cv2pcl(rotatedPoints, pclPoints);

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> pointsColor(pclPoints.makeShared(), color[2], color[1], color[0]);
  viewer->addPointCloud<pcl::PointXYZ>(pclPoints.makeShared(), pointsColor, title);
}
void PinholeCamera::projectPoints(const std::vector<cv::Point3f> &points, const PoseRT &pose_cam, std::vector<cv::Point2f> &projectedPoints) const
{
  PoseRT fullPose = extrinsics * pose_cam;
  cv::projectPoints(Mat(points), fullPose.getRvec(), fullPose.getTvec(), cameraMatrix, distCoeffs, projectedPoints);
}