void CCalibrateKinect::undistortImages ( const std::vector< cv::Mat >& vImages_, const cv::Mat_<double>& cvmK_, const cv::Mat_<double>& cvmInvK_, const cv::Mat_<double>& cvmDistCoeffs_, std::vector< cv::Mat >* pvUndistorted_ ) const
{
std::cout << "undistortImages() "<< std::endl << std::flush;
CHECK ( !vImages_.empty(), "undistortImages(): # of undistorted images can not be zero.\n" );
CHECK ( !cvmK_.empty(), "undistortImages(): K matrix cannot be empty.\n" );
CHECK ( !cvmInvK_.empty(), "undistortImages(): inverse of K matrix cannot be empty.\n" );
CHECK ( !cvmDistCoeffs_.empty(), "undistortImages(): distortion coefficients cannot be empty.\n" );
cv::Size cvFrameSize = vImages_[0].size(); //x,y;
pvUndistorted_->clear();
for ( unsigned int n = 0; n < vImages_.size(); n++ )
{
cv::Mat cvUndistorted;
std::cout << "distort: "<< n << "-th image.\n"<< std::flush;
undistortImage ( vImages_[n], cvmK_, cvmInvK_, cvmDistCoeffs_, &cvUndistorted );
pvUndistorted_->push_back ( cvUndistorted );
//string strNum = boost::lexical_cast< std::string> ( n );
//string strRGBUndistortedFileName = "rgbUndistorted" + strNum + ".bmp";
//cv::imwrite ( strRGBUndistortedFileName, cvUndistorted );
}
return;
}
void PinholeUndistorter<DISTORTION_T, MASK_T>::constructUndistortedFrame(
const cv::Mat & image, frame_t & outFrame) const {
SM_ASSERT_TRUE(std::runtime_error, _idealGeometry,
"Camera has not yet been set.")
cv::Mat undistImage;
undistortImage(image, undistImage);
outFrame.setImage(undistImage);
outFrame.setGeometry(_idealGeometry);
}
bool NaoInput::getFrame(Frame &frame)
{
std::string cameraTop = "CameraTop";
int space = 1;
std::vector<float> newCameraPosition = this->motProxy->getPosition(cameraTop, space, true);
//std::vector<float> relativeCameraPosition;
//for (int i=0; i<6 ; i++)
//{
// relativeCameraPosition.push_back( newCameraPosition[i] - initialCameraPosition[i] );
//}
frame.camPosition = newCameraPosition;
// get the image from camera
cv::Mat imgHeader = cv::Mat(cv::Size(640, 480), CV_8UC3);
AL::ALValue img = camProxy->getImageRemote(clientName);
imgHeader.data = (uchar*) img[6].GetBinary();
camProxy->releaseImage(clientName);
undistortImage(imgHeader, cameraMatrix, distortionCoeffs);
frame.img = imgHeader.clone();
return true;
}
