cv::undistort returns corrupt image

edit

Hi

original image:

image after undistort:

my code:

double * matrix_rgb = new double[9];
matrix_rgb[0] = fx_rgb;     matrix_rgb[1] = 0;          matrix_rgb[2] = cx_rgb;
matrix_rgb[3] = 0;          matrix_rgb[4] = fy_rgb;     matrix_rgb[5] = cy_rgb;
matrix_rgb[6] = 0;          matrix_rgb[7] = 0;          matrix_rgb[8] = 1;

double * vector_distorsion_rgb = new double[5];
vector_distorsion_rgb[0] = k1_rgb;
vector_distorsion_rgb[1] = k2_rgb;
vector_distorsion_rgb[2] = p1_rgb;
vector_distorsion_rgb[3] = p2_rgb;
vector_distorsion_rgb[4] = k3_rgb;

mCameraMatrixRGB     = new cv::Mat(3, 3, CV_64FC1, matrix_rgb);
mDistortionCoeffsRGB = new cv::Mat(1, 5, CV_64FC1, vector_distorsion_rgb);
cv::Mat srcMat_rgb(cv::Size(image_RGB->dimX, image_RGB->dimY), CV_8UC3, image_RGB->data, cv::Mat::AUTO_STEP);
cv::Mat undistorted;

cv::undistort(srcMat_rgb, undistorted, *mCameraMatrixRGB, *mDistortionCoeffsRGB);

for(int i = 0; i <image_RGB->dimX*image_RGB->dimY*image_RGB->planes; i++){
    ((uchar*)image_RGB->data)[i] = undistorted.data[i];
}

i hope some one can tell me what im doing wrong :(