本文整理汇总了C++中image_geometry::PinholeCameraModel::rectifyPoint方法的典型用法代码示例。如果您正苦于以下问题：C++ PinholeCameraModel::rectifyPoint方法的具体用法？C++ PinholeCameraModel::rectifyPoint怎么用？C++ PinholeCameraModel::rectifyPoint使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类image_geometry::PinholeCameraModel的用法示例。
在下文中一共展示了PinholeCameraModel::rectifyPoint方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: getCameraRay
void getCameraRay(const image_geometry::PinholeCameraModel& model, const cv::Point2d& pt, cv::Point3d* ray)
{
cv::Point2d rect_point;
rect_point = model.rectifyPoint(pt);
ROS_DEBUG("Rect Point: %f, %f",rect_point.x,rect_point.y);
*ray = model.projectPixelTo3dRay(rect_point);
}示例2: imageCb
void imageCb(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg) {
MyFirst.clear();
Ccount1++;
cv_bridge::CvImageConstPtr cv_ptr;
double cur = ros::Time::now().toSec();
// lpf_x.set_cutoff_freq(0);
// lpf_x.get_lpf(10);
try
{
cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cam_model_left.fromCameraInfo(info_msg);
// cout << "dist " << info_msg->D[0] << " " << info_msg->D[1] << " " << info_msg->D[2] << " " << endl;
Mat image, gray, temp, mask;
image = cv_ptr->image;
mask = image.clone();
int niters = 1;
dilate(mask, temp, Mat(), Point(-1, -1), niters);
erode(temp, temp, Mat(), Point(-1, -1), niters * 2);
dilate(temp, temp, Mat(), Point(-1, -1), niters);
threshold(temp, temp, 200, 255, CV_THRESH_BINARY);
for (int i = 0; i <= 5; i++) {
for (int j = 1000; j < temp.cols; j++) {
temp.at<uchar>(i, j) = 0;
}
}
vector<vector<Point> > contours1, contours2;
vector<Vec4i> hierarchy;
findContours(temp, contours2, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
int cmin = 0;
int cmax = 100;
for (int i = 0; i < contours2.size(); i++) {
if (contours2[i].size() < cmin || contours2[i].size() > cmax) {
continue;
}
else
contours1.push_back(contours2[i]);
}
image = Scalar(255, 255, 255);
std::vector<std::vector<cv::Point> > ::iterator itc;
itc = contours1.begin();
MyPoint TTEMP;
while (itc != contours1.end()) {
cv::Moments mom = cv::moments(cv::Mat(*itc++));
#ifdef DEBUG
cv::circle(image, cv::Point(mom.m10 / mom.m00, mom.m01 / mom.m00), 2, cv::Scalar(0), 2);
#endif
TTEMP.x = mom.m10 / mom.m00;
TTEMP.y = mom.m01 / mom.m00;
MyFirst.push_back(TTEMP);
}
#ifdef DEBUG
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
//Rect rect(MyFirst[0].x - 50, MyFirst[0].y- 50, 100, 100);
//Mat subimage = cv_ptr->image(rect);
//cv::imshow(OPENCV_WINDOW, subimage);
cv::waitKey(3);
#endif
vector<My3DPoint> myvec;
My3DPoint myTempVec;
#ifdef DEBUG
//cout << Ccount << " " << MyFirst[0].y << " " << MySecond[0].y << endl;
#endif
// cout << "size : " << MyFirst.size() << "," << MySecond.size() << endl;
if (MyFirst.size() && MySecond.size()) {
// cout << MyFirst[0].x << "," << MyFirst[0].y << endl;
cv::Point2d left_pt(MyFirst[0].x, MyFirst[0].y);
cv::Point2d right_pt(MySecond[0].x, MySecond[0].y);
cam_model_left.rectifyPoint(left_pt);
Matrix<float, 3, 1> Z;
Matrix<float, 6, 1> RES_KALMAN;
Z<<
//.........这里部分代码省略.........