Inline undistorting an image

edit

Hello,

I have used the OpenCV Calibration functions to create my distortion coefficients along with the camera matrix. Now, I want to take the distortion coefficients along with camera matrix to undistort a specific columns and rows in a random fashion. The reason for this is that I take the original distorted imagine and calculate a single line of columns such that I take all pixel values in a given column to arrive at a specific row weighted average.

From this, I want to remove the distortion of just this single line of X and Y(values). What specific calculation do I need to do to get this undistorted result? I don't need to go back to image pixel space, therefore everything can remain in floating point. I have tried the following, however I believe is production the distorted values because the resulting curve is in the wrong direction.

Find code snippet below: I used the equation that is contained in initundistortrectifymap removing the rectify aspects and unsuded K3,k4,5 and such.

void __fastcall TCamera::ProcessImage(mvIMPACT::acquire::ImageBuffer * ABuffer)
{
    //Member function accepts an input image buffer and calculates a measure buffer
    //The measure buffer is width*4*2, where each column contains both an row
    //sum average and a sum intensity.
    TBufferFrame * LBuffer;

    double LDistortedX,LDistortedY;
    //Distortion removal elements
    double Lx;
    double Ly;
    double Lxy;
    double LSqrX;
    double LSqrY;
    double LSqrD;       //Square Distance (^2) also known as r2
    double LSqrSqrD;    //Distance (^4)
    double LDestX;
    double LDestY;
    double LInvDen;
    double Lkr;
    double Lp1,Lp2;
    double Lk1,Lk2;
    double Lu0,Lv0;
    double Lfx,Lfy;

    unsigned __int32 LRowOffset;
    unsigned __int32 LRow;
    unsigned __int32 LColumn;
    unsigned __int32 LWidth;
    unsigned __int32 LHeight;
    unsigned __int32 LOffset;
    unsigned __int32 LXSum;
    unsigned __int32 LIntensity;
    unsigned __int32 LData;

    union
    {
        void * LRawData;
        unsigned __int8 * LByteData;
        unsigned __int16 * LWordData;
        unsigned __int32 * LDwordData;
    };

    if(ABuffer!=NULL)
    {
        LXSum = 0;
        LIntensity = 0;
        LRowOffset = ABuffer->iWidth;
        LWidth = ABuffer->iWidth;
        LHeight = ABuffer->iHeight;


        LBuffer = FFrameManager->NewFrame(FFrameId);
        if(LBuffer==nullptr)
            return;

        LRawData = ABuffer->vpData;

        Lk1 = FLensModel.R[0];  //Radial coefficient 1 (k1)
        Lk2 = FLensModel.R[1];  //Radial coefficient 2 (k2)
        Lp1 = FLensModel.T[0];  //Tangental coefficient 1 (p1)
        Lp2 = FLensModel.T[1];  //Tangental coefficient 2 (p2)
        Lu0 = FLensModel.Center[0]; //Optical center X
        Lv0 = FLensModel.Center[1]; //Optical center y
        Lfx = FLensModel.Fx;    //Camera Focal Fx
        Lfy = FLensModel.Fy;    //Camera Focal Fy

        //First,calculate column data as XSum and Intensity.
        for(LColumn=0;LColumn<LWidth;LColumn++)
        {
            LXSum = 0;
            LIntensity = 0;

            for(LRow=0;LRow<LHeight;LRow++)
            {
                //Assume data is mono8 for now.
                LOffset = LRowOffset*LRow + LColumn;
                LData = LByteData[LOffset];
                if(LData>10)    //Threshold of 10
                {
                    LXSum += (LRow+1)*LData;
                    LIntensity += LData;
                }
            }

            //Now, calibration the result and put into the measure record.
            if(LIntensity<50)
            {
                LBuffer->Data[LColumn].Range = 0;
                LBuffer->Data[LColumn].X = 0;
            }
            else
            {
                //Calculate the range and x.
                LDistortedX = LColumn;
                LDistortedY = (double)LXSum/(double)LIntensity;

                Lx = (LDistortedX - Lu0)/Lfx;
                Ly = (LDistortedY - Lv0)/Lfy;
                Lxy = Lx*Ly;
                LSqrX = Lx*Lx;
                LSqrY = Ly*Ly;
                LSqrD = LSqrX+LSqrY;
                LSqrSqrD = LSqrD*LSqrD;
                Lkr = (1.0l +((0*LSqrD + Lk2)*LSqrD + Lk1)*LSqrD);
                LDestX = Lfx*(Lx*Lkr + Lp1 ...