Calibrate Camer using OpenCV in ROS [closed]

edit

Hi all. I am writing code to subscribe raw video frame and calibrate them as well as publish. I was using OpenCV to generate .xml file for camera calibration as expected. I also calibrate the video in OpenCV succesfully by reading "cameraMatrix" and "distCoeffs", getting the image using "videocapture" and watching the image in "imshow" . The OpenCV function I used is "undistort(old_frame, new_frame, cameraMatrix, distCoeffs)".

All things works excellently but when I change it into ROS, I can only get a heavily distorted image and don't know why.

Here is my code

#include "CalibratedVideo.h"
#include <cv_bridge/cv_bridge.h>

using namespace cv;
using namespace std;


CalibratedVideo::CalibratedVideo(ros::NodeHandle n){
image_transport::ImageTransport it(n);
img_sub = it.subscribe("/usb_cam/image_raw", 1, &CalibratedVideo::send_video, this);
img_pub = it.advertise("/CalibratedVideo/image_fine",1);


//read the calibrating parameters.
string path = ros::package::getPath("irb120_cam_cali");
const string inputSettingsFile = path + "/src/out_camera_data.xml";
ROS_INFO("%s\n", inputSettingsFile.c_str());
FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
if (!fs.isOpened())
{
    ROS_INFO("Could not open the configuration file");
    return;
}
fs["Camera_Matrix"] >> cameraMatrix;
fs["Distortion_Coefficients"] >> distCoeffs;
fs.release();


}

void CalibratedVideo::send_video(const sensor_msgs::ImageConstPtr& img){
Mat newFrame;
cv_bridge::CvImagePtr inMsgPtr;
inMsgPtr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::BGR8);
undistort(inMsgPtr->image, newFrame, cameraMatrix, distCoeffs);
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", newFrame).toImageMsg();
img_pub.publish(msg);
ROS_INFO("sent frame width=%d height=%d", newFrame.cols, newFrame.rows);
}

int main (int argc, char* argv[]){
ros::init(argc, argv, "cali");
ros::NodeHandle nh;
CalibratedVideo cali(nh);
ros::spin();

return 0;}