35 #ifndef _CLASS_PINHOLE_PROJECTION_CPP_
36 #define _CLASS_PINHOLE_PROJECTION_CPP_
65 printf(
"cvMat has %d cols and %d lines\n", mat->cols, mat->rows);
69 printf(
"%s has %d cols and %d lines\n",name, mat->cols, mat->rows);
72 for (
int l=0; l<mat->rows; l++)
75 for (
int c=0;
c<mat->cols;
c++)
77 printf(
"%3.2f ", mat->at<
double>(l,
c));
87 cv::Mat vertexes(4, (
int)v->points.size(), CV_64FC1);
88 cv::Mat pixels(3, (
int)v->points.size(), CV_64FC1);
91 for (
size_t i=0; i < v->points.size(); i++)
93 vertexes.at<
double>(0,i) = v->points.at(i).x;
94 vertexes.at<
double>(1,i) = v->points.at(i).y;
95 vertexes.at<
double>(2,i) = v->points.at(i).z;
96 vertexes.at<
double>(3,i) = 1;
101 transformation.create(4,4,CV_64F);
106 pixels = transformation*vertexes;
110 p->erase(p->begin(), p->end());
114 for (
int i=0; i < pixels.cols; i++)
117 pt.x = (pixels.at<
double>(0,i) / pixels.at<
double>(2,i));
118 pt.y = (pixels.at<
double>(1,i) / pixels.at<
double>(2,i));
156 transformation.create(4,4,CV_64F);
160 int valid_projections=0;
163 double p11 = M->at<
double>(0,0);
double p12 = M->at<
double>(0,1);
double p13 = M->at<
double>(0,2);
double p14 = M->at<
double>(0,3);
164 double p21 = M->at<
double>(1,0);
double p22 = M->at<
double>(1,1);
double p23 = M->at<
double>(1,2);
double p24 = M->at<
double>(1,3);
165 double p31 = M->at<
double>(2,0);
double p32 = M->at<
double>(2,1);
double p33 = M->at<
double>(2,2);
double p34 = M->at<
double>(2,3);
171 double K1 = (C*p12*p34+p13*D*p32-p12*p33*D-p32*C*p14-p13*B*p34+p33*B*p14);
172 double K2 = (p32*C*p24-p34*C*p22-p33*B*p24-p32*p23*D+p33*D*p22+p34*p23*B);
173 double K3 = (-p13*D*p22+C*p14*p22-C*p12*p24+p12*p23*D-p14*p23*B+p13*B*p24);
174 double K4 = (C*p31*p12+p11*p33*B+p13*A*p32-C*p11*p32-p33*A*p12-p31*p13*B);
175 double K5 = (C*p21*p32+p31*p23*B+p33*A*p22-C*p31*p22-p23*A*p32-p21*p33*B);
176 double K6 = (-C*p21*p12-p11*p23*B+C*p11*p22+p23*A*p12+p21*p13*B-p13*A*p22);
177 double K7 = (p11*p34*C-p13*A*p34-p11*p33*D+p33*A*p14+p31*p13*D-p31*C*p14);
178 double K8 = (-p33*A*p24-p31*p23*D+p23*A*p34+p21*p33*D-C*p21*p34+C*p31*p24);
179 double K9 = (C*p21*p14+p13*A*p24-p23*A*p14-p21*p13*D+p11*p23*D-C*p11*p24);
180 double K10 = (C*p31*p12+p11*p33*B+p13*A*p32-C*p11*p32-p33*A*p12-p31*p13*B);
181 double K11 = (C*p21*p32+p31*p23*B+p33*A*p22-C*p31*p22-p23*A*p32-p21*p33*B);
182 double K12 = (-C*p21*p12-p11*p23*B+C*p11*p22+p23*A*p12+p21*p13*B-p13*A*p22);
185 v->points.erase(v->points.begin(), v->points.end());
188 cv::Mat XYZ_plane(4, 1, CV_64FC1);
189 XYZ_plane.at<
double>(3,0) = 1;
190 cv::Mat XYZ_image(4, 1, CV_64FC1);
193 for (
int i=0; i<(int)p->points.size();i++)
195 pcl::PointXYZRGB pix = p->at(i);
197 pt.x = -(K1*pix.y + K2*pix.x + K3) / (K4*pix.y + K5*pix.x + K6);
198 pt.y = (K7*pix.y + K8*pix.x + K9) / (K10*pix.y + K11*pix.x + K12);
199 pt.z = -(A*pt.x + B*pt.y + D)/C;
202 XYZ_plane.at<
double>(0,0) = pt.x;
203 XYZ_plane.at<
double>(1,0) = pt.y;
204 XYZ_plane.at<
double>(2,0) = pt.z;
205 XYZ_image = extrinsic*XYZ_plane;
207 if (XYZ_image.at<
double>(2,0)>0)
211 v->points.push_back(pt);
219 v->points.push_back(pt);
225 ROS_INFO(
"Projecting %d pixels. Valid projections = %d", (
int)p->size(), valid_projections);
227 return valid_projections;};
232 transformation.create(4,4,CV_64F);
236 int valid_projections=0;
239 double p11 = M->at<
double>(0,0);
double p12 = M->at<
double>(0,1);
double p13 = M->at<
double>(0,2);
double p14 = M->at<
double>(0,3);
240 double p21 = M->at<
double>(1,0);
double p22 = M->at<
double>(1,1);
double p23 = M->at<
double>(1,2);
double p24 = M->at<
double>(1,3);
241 double p31 = M->at<
double>(2,0);
double p32 = M->at<
double>(2,1);
double p33 = M->at<
double>(2,2);
double p34 = M->at<
double>(2,3);
247 double K1 = (C*p12*p34+p13*D*p32-p12*p33*D-p32*C*p14-p13*B*p34+p33*B*p14);
248 double K2 = (p32*C*p24-p34*C*p22-p33*B*p24-p32*p23*D+p33*D*p22+p34*p23*B);
249 double K3 = (-p13*D*p22+C*p14*p22-C*p12*p24+p12*p23*D-p14*p23*B+p13*B*p24);
250 double K4 = (C*p31*p12+p11*p33*B+p13*A*p32-C*p11*p32-p33*A*p12-p31*p13*B);
251 double K5 = (C*p21*p32+p31*p23*B+p33*A*p22-C*p31*p22-p23*A*p32-p21*p33*B);
252 double K6 = (-C*p21*p12-p11*p23*B+C*p11*p22+p23*A*p12+p21*p13*B-p13*A*p22);
253 double K7 = (p11*p34*C-p13*A*p34-p11*p33*D+p33*A*p14+p31*p13*D-p31*C*p14);
254 double K8 = (-p33*A*p24-p31*p23*D+p23*A*p34+p21*p33*D-C*p21*p34+C*p31*p24);
255 double K9 = (C*p21*p14+p13*A*p24-p23*A*p14-p21*p13*D+p11*p23*D-C*p11*p24);
256 double K10 = (C*p31*p12+p11*p33*B+p13*A*p32-C*p11*p32-p33*A*p12-p31*p13*B);
257 double K11 = (C*p21*p32+p31*p23*B+p33*A*p22-C*p31*p22-p23*A*p32-p21*p33*B);
258 double K12 = (-C*p21*p12-p11*p23*B+C*p11*p22+p23*A*p12+p21*p13*B-p13*A*p22);
261 v->points.erase(v->points.begin(), v->points.end());
264 cv::Mat XYZ_plane(4, 1, CV_64FC1);
265 XYZ_plane.at<
double>(3,0) = 1;
266 cv::Mat XYZ_image(4, 1, CV_64FC1);
269 for (
int i=0; i<(int)p->size();i++)
271 pcl::PointXY pix = p->at(i);
273 pt.x = -(K1*pix.y + K2*pix.x + K3) / (K4*pix.y + K5*pix.x + K6);
274 pt.y = (K7*pix.y + K8*pix.x + K9) / (K10*pix.y + K11*pix.x + K12);
275 pt.z = -(A*pt.x + B*pt.y + D)/C;
278 XYZ_plane.at<
double>(0,0) = pt.x;
279 XYZ_plane.at<
double>(1,0) = pt.y;
280 XYZ_plane.at<
double>(2,0) = pt.z;
281 XYZ_image = extrinsic*XYZ_plane;
283 if (XYZ_image.at<
double>(2,0)>0)
286 v->points.push_back(pt);
293 v->points.push_back(pt);
299 ROS_INFO(
"Projecting %d pixels. Valid projections = %d", (
int)p->size(), valid_projections);
302 return valid_projections;}
struct class_pinhole_projection::@1 projection_plane
CGAL::Cartesian< double > K1
int project_pixel_to_vertex(const std::vector< pcl::PointXY > *p, pcl::PointCloud< pcl::PointXYZ > *v)
int set_projection_plane(double a, double b, double c, double d)
Sets and gets.
int project_pixel_with_color_to_vertex(const pcl::PointCloud< pcl::PointXYZRGB > *p, pcl::PointCloud< pcl::PointXYZRGB > *v)
header for pinhole projection. Defines default distortions parameters for mit dataset cameras...
int project_vertex_to_pixel(const pcl::PointCloud< pcl::PointXYZ >::Ptr v, std::vector< pcl::PointXY > *p)
Projections.
int print_CvMat(cv::Mat *mat, const char *name)
Prints.