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);
}