how to reproject world origin back to image

[flankechen]

Thanks for sharing the code! @raulmur I am trying to reproject world origin(surface) back to every frame.

I copy the world to camera matrix by

if(pTracker->mLastProcessedState==Tracking::WORKING)
{
    mTransfromW2C = pTracker->mCurrentFrame.mTcw.clone();

    mRotationW2C = mTransfromW2C.rowRange(0,3).colRange(0,3);
    mTranslationW2C = mTransfromW2C.rowRange(0,3).col(3);

    //cout<<"mTranslationW2C:"<<mTranslationW2C<<endl;
    //cout<<"mRotationW2C:"<<mRotationW2C<<endl;
}

and then project world surface back the image

vector<cv::Point3f> objectPoints; // 3D points in world coordinate
vector<cv::Point2f> imagePoints; // 2D points in image

int nHalfCells = 1;
int nTot = nHalfCells * 2 +1;

for(int i=0; i<nTot; i++)
    for(int j=0; j<nTot; j++)
    {
        cv::Point3f point3f((i - nHalfCells)*0.01 , (j - nHalfCells)*0.01 , 0.0);
        objectPoints.push_back(point3f);
        cout<<"object points"<<point3f<<endl;
    }



if(!mTransfromW2C.empty())
{
    //cout<<"C2W matrix:"<<mTransfromC2W<<endl;

    cv::Mat rVec(3, 1, cv::DataType<double>::type); // Rotation vector
    cv::Rodrigues(mRotationW2C, rVec);

    cv::projectPoints(objectPoints,rVec,mTranslationW2C, mK, mDistCoef, imagePoints);

    for(int i = 0; i < imagePoints.size(); i++)
    {
        cout<<"image points"<<imagePoints[i]<<endl;
        cv::circle(im, imagePoints[i], 5, cv::Scalar(255,0,0),-1);
    }
}

I got success in initialization, However, undesired results like: object points [-0.01, -0.01. 0] --> image points [88735.8, -58801.5] object points [-0.01, 0, 0] --> image points [64352.4, -26369.5] object points [-0.01, 0.01, 0] --> image points [54829.8, -8205.73] object points [0, -0.01, 0] --> image points [342763, -177354] object points [0,0,0] --> image points [297306, -177354]

the tracking and maps looks good. the scale of the object points might not be the problem, when I could see the points in the image, the position and transform is not right.

how is "world" define? and what am I missing.

Thanks!