camera_info_publisher.cpp:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
#include <camera_info_manager/camera_info_manager.h> int main(int argc, char** argv) { ros::init(argc, argv, "camera_info_publisher"); ros::NodeHandle nh; const std::string cname="prosilica"; const std::string url="file://${ROS_HOME}/camera_info/prosilica.yaml"; ros::Publisher pubCameraInfo = nh.advertise<sensor_msgs::CameraInfo>("camera/camera_info", 1); camera_info_manager::CameraInfoManager cinfo(nh,cname,url); sensor_msgs::CameraInfo msgCameraInfo; ros::Rate loop_rate(5); while (nh.ok()) { msgCameraInfo=cinfo.getCameraInfo(); pubCameraInfo.publish(msgCameraInfo); ros::spinOnce(); loop_rate.sleep(); } } |
image_geometry_demo.cpp:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
#include <ros/ros.h> #include <image_geometry/pinhole_camera_model.h> void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& info_msg) { std::cout<<"===============================cameraInfoCallback===============================" <<std::endl; image_geometry::PinholeCameraModel cam_model_; cam_model_.fromCameraInfo(info_msg); // projection_matrix is the matrix you should use if you don't want to use project3dToPixel() and want to use opencv API cv::Matx34d projection_matrix=cam_model_.fullProjectionMatrix(); std::cout<<cam_model_.project3dToPixel(cv::Point3d(-0.1392072,-0.02571392, 2.50376511) )<<std::endl; } int main(int argc,char ** argv) { ros::init(argc,argv,"image_geometry_demo",1); ros::NodeHandle nh; ros::Subscriber sub = nh.subscribe("camera/camera_info", 1000, cameraInfoCallback); ros::spin(); } |
CMakeLists.txt
|
1 |
find_package(catkin REQUIRED COMPONENTS image_geometry camera_info_manager roscpp) |
