what is the unit of the camera pose information?

[QQQYang]

Hello, I want to use the camera position information, which is geometry_msgs/PoseStamped message type. But I am confused with the unit of the position.

[Severn-Vergil]

http://www.ros.org/reps/rep-0103.html

[AlejandroSilvestri]

Qian Yang,

If you are using monocular orb slam, know that it's impossible for the system to know the real world scale only from one camera. So orb slam initialize the map with an arbitrary scale, that varies on each initialization.

See #174

[salsicha]

Hi, I'm new to ORBSLAM. Just to clarify the above answers: I've run stereo slam and there is an output file "FrameTrajectory_KITTI_Format.txt." What is the scale of the units? Arbitray? Meters? Thanks.

Also, I'm using rectified images, and the KITTI04-12.yaml settings file from Examples/Stereo. I would like to know if these settings match my camera (ZED). I assume if the settings are wrong then the scale and trajectory must also be wrong. Is that a correct assumption? Again, thanks.

[AlejandroSilvestri]

Hi @salsicha Tudo bem?

Scale is arbitrary in monocular, but in stereo it depends on camera structure, the distance between them.

You are right when assuming that bad settings will give you bad scale and trajectory. You must edit yaml settings file, and put your camera's parameters.

[lordofcoders]

Hi, I saved the pose matrixes returned from "TrackMonocular" function to a file whenever tracking status was "OK". Although the visualization of point cloud and camera path seem correct during the program execution, I noticed that the first saved pose had rotation matrix that wasn't identity and had nonzero translation. Shouldn't be the first pose reference frame itself? Also here is the first column of one of the rotation matrixes, namely X axis of it. 2.91361e-19 0.0779774 1.22311e-16 shouldn't it suppose to have a magnitude of 1? or do the axises have a scaling relative to the reference frame? Also is the coordinate frame right handed or left handed? Thank you!

[AlejandroSilvestri]

@lordofcoders

Tracking::mInitialFrame::mTcw has the initial pose. You start receiving poses after initialization, which never is the initial pose, because initialization need two poses, the second one is the current one when initialization finishes and status is ok.

And you are right, rotation matrix should have a magnitude of 1. Where did you get that rotation matrix column?

[lordofcoders]

These are my first 3 pose matrixes

-2.947e-24 8.01443e-17 0.00781198 9.49551e-22 0.00781198 9.49551e-22 1.32791e-22 -1.51037e-21 1.32791e-22 -1.51037e-21 0 0.0078125

-9.76013e-30 2.502e-16 0.00781234 1.35465e-22 0.00781234 1.35465e-22 3.99437e-24 -2.43302e-20 3.99437e-24 -2.43302e-20 0 0.0078125

-2.71274e-27 6.42456e-16 0.0078122 3.443e-22 0.0078122 3.443e-22 3.13858e-23 -9.2544e-20 3.13858e-23 -9.2544e-20 0 0.0078125 obtained by following ros callback where "srPoses" is the string stream that will be finally written to a file

void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvShare(msg);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
        cv::Mat pose;
        pose=mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
    save_mutex.lock();
    if(willSave)
    {   
        std::cout<<"trying to save..."<<std::endl;
        if(mpSLAM->GetTrackingState()==2)
        {
            std::stringstream srImageName;
            srImageName<<savedImageCount<<".jpg";

            if(cv::imwrite(srImageName.str(),  cv_ptr->image))
            {
            std::cout<<"saved image:"<<srImageName.str()<<std::endl;
            ofsImages<<srImageName.rdbuf()<<std::endl;
            for(int i=0; i<3;i++)
            {   
               for(int j=0; j<3;j++)
               {
                srPoses<<pose.at<double>(i,j)<<" ";
               }
                srPoses<<pose.at<double>(i,3)<<std::endl;
            }
            savedImageCount++;
            }
        }
    }
    save_mutex.unlock();
    callBackCount++;
}

[AlejandroSilvestri]

@lordofcoders

I see. A pose matrix is a 4x4 one. You are missing the last row. I believe it is 0, 0, 0, 0.0078125 in those examples.

Divide all elements in each matrix by the last element (fourth row, fourth column). In these examples try 0.0078125. Then you can grab your rotation submatrix and translation subvector.

[lordofcoders]

Hi @AlejandroSilvestri , thank you for this valuable feedback. I printed out the entire pose matrixes by modifying the for loop of above code as follows, but the 4th row contains really weird values.

//My previous comment includes the rest of the code
            for(int i=0; i<4;i++)
            {   
               for(int j=0; j<3;j++)
               {
                srPoses<<pose.at<double>(i,j)<<" ";///
               }
                srPoses<<pose.at<double>(i,3)<<std::endl;
            }

here are the first 3 poses generated from this code

-1.24251e-22 -1.25581e-17 0.00781212 8.61216e-27 0.00781212 8.61216e-27 -8.14645e-29 -3.58507e-31 -8.14645e-29 -3.58507e-31 0 0.0078125 0 0.0078125 5.53354e-322 4.00193e-322

-2.05969e-20 -2.05784e-16 0.00781098 -4.477e-31 0.00781098 -4.477e-31 -3.80651e-24 6.13686e-33 -3.80651e-24 6.13686e-33 0 0.0078125 0 0.0078125 2.12412e-311 4.27367e-321

-3.14317e-19 -7.55884e-16 0.00780963 -2.20978e-29 0.00780963 -2.20978e-29 -1.03137e-23 6.66361e-23 -1.03137e-23 6.66361e-23 0 0.0078125 0 0.0078125 5.53354e-322 4.79244e-322

[AlejandroSilvestri]

@lordofcoders ,

Something is very wrong. Are you sure you are getting mTcw? Where does pose come from?

frames mTcw always end in 0,0,0,1

[lordofcoders]

here is my call hierarchy System::TrackMonocular Tracking::GrabImageMonocular return mCurrentFrame.mTcw.clone();

[AlejandroSilvestri]

@lordofcoders

mInitialFrame.mTcw is asigned the unit matrix. See Tracking.cc, at the end of MonocularInitialization().

My own test:

mCurrentFrame.mTcw: [0.93915111, 0.015685702, -0.34314597, 0.4722206; -0.036274884, 0.99789989, -0.053664736, 0.039145086; 0.34158355, 0.062846877, 0.93774778, 0.07891535; 0, 0, 0, 1]

[RichardFieb]

@lordofcoders We had the same problem. using float instead of double should solve it.

[arashazimi0032]

hi @AlejandroSilvestri. I am trying to implement the IND (https://discovery.ucl.ac.uk/id/eprint/1369912/) method developed by Dr. Hosseini nave in order to select a keyframe in ORB-SLAM3 algorithm. for that i need to know the camera pose. I have the homography matrix in the mTcw variable but I do not know how to get the position of each frame from that. Because I have to be able to get the angle between normal to the surface at any point and the Camera to Point connector vector. Do you initialize the starting position with a value of zero? in the ORBmatcher.cc and in "SearchByProjection" function, the Viewing angle is calculated with the following Code:

cv::Mat sRcw = Scw.rowRange(0,3).colRange(0,3); const float scw = sqrt(sRcw.row(0).dot(sRcw.row(0))); cv::Mat Rcw = sRcw/scw; cv::Mat tcw = Scw.rowRange(0,3).col(3)/scw; cv::Mat Ow = -Rcw.t()*tcw;

for(int iMP=0, iendMP=vpPoints.size(); iMP<iendMP; iMP++) { MapPoint* pMP = vpPoints[iMP];

    // Get 3D Coords.
    cv::Mat p3Dw = pMP->GetWorldPos();

    cv::Mat PO = p3Dw-Ow;
    const float dist = cv::norm(PO);

    // Viewing angle must be less than 60 deg
    cv::Mat Pn = pMP->GetNormal();

    if(PO.dot(Pn)<0.5*dist)
        continue;

} what is the Ow vector in the Code above?? Is Ow the absolute position of the camera in the world coordinate?? When I want to use the above code to get the angle between normal to the surface at any point and the Camera to Point connector vector, the value inside the arccos arc (that's mean (PO.dot(Pn))/dist) is equal to 1 and the answer will be nan ::((. What should I do? thanks.

[AlejandroSilvestri]

@arashazimi0032

I know about orb_slam2, let's assume orb_slam3 is the same thing.

• Yes, Ow means the camera center position in the world reference
• No, mTcw is not an homography 3x3 matrix, it's an euclidean transformation in homogeneous coordinates 4x4 matrix. It transforms from world coordinates to camera coordinates
• The normal to the point is not exactly the normal to the surface. If I remember correctly, It's like an average of its viewing directions.

If you want to get the angle, prior to evaluate arccos(argument) you should ceil the argument like:

if(argument>1.0)
    argument = 1.0;
float angle = arcos(argument);

[arashazimi0032]

Thank you very much for your help @AlejandroSilvestri So I have to use the PCL library for compute the normal to the surface. thanks again.