Publish camera pose as tf ros message

[Thomas00010111]

Ros rgbd example:

• Transforms camera pose into right handed frame
• Transform camera frame into NED body frame (x forwards, y right, z down)
• publishes pose as tf message

[mikjh]

Quick question if you could help. How to get the world position from this transform? The translation is not following the view-direction of the camera, and I couldn't find any sensible multiplication to make it right.

[Thomas00010111]

Haven't used it for quite a while, but the node should be:
"tf/camera_pose/pose"

It is the pose of the camera in world frame, start position is 0,0,0. It worked for me but when you find any errors let me know.

[dannyway03]

Hi. I'm newbie in ROS, and i have some questions.
Just to understand, could you explain me how you worked out the matrix transformations?

I was trying to display in rviz (camera display) the current camera image in overlay to the point cloud map. Everything is somehow working, however there seems to be a mismatch between the map points pose and published /tf (which imposes the displayed image pose). As a result, the map overlay is basically wrong.
Have you ever tried something like this?
Or have any suggestion?

thank you
-d

[Thomas00010111]

Hi, I just checked again and found out that what I did is wrong. Currently I am correcting the code. I will update the pull request in the upcoming days.

[Thomas00010111]

Updated. /tf/camera_pose returns distance from origin.

[ruilimit]

Does /tf/camera_pose also apply to the stereo cameras instead of rgbd? Thanks!

[Thomas00010111]

I have never tried Orb Slam 2 with a stereo camera, so I do not know. I can imagine that you can apply the same transformation to each of the camera.

[kpach]

At the 146th line, are you sure that the transformation is correct? I don't think you should apply the rotation to the translation part, as [R, t;0 0 0 1]_[R0, 0;0 0 0 1] = [R_R0, t;0 0 0 1]. Please correct me if I am wrong.

Even then, the translation part doesn't seem to work with my RGBD camera. All X,Y,Z components have the value below 10^{-4}. Could you give me any guide to solve this issue?

Thanks,

[Thomas00010111]

For me it worked ok. I have a camera mounted on a quadcopter and it gives me the translation in x,y,z direction. The origin(0,0,0) is where the algorithm was started. I also tried turning the camera and then translating it and the output was what I had expected. So the output was definitely not 10^4 or similar, but a translation from the origin in x,y,z direction, in cm.

What the line does is changing the sequence of coordinates in the vector, xyz becomes zxy. So I guess the multiplication is necessary. But I struggled quite a bit to get the rotations right, so I would not be surprised if there is still a bug somewhere.

Is Orb Slam running correctly? Do you see that the pointcloud is created?

[kpach]

EDITED#2: Pointcloud is created. (my bad)

The pointcloud is created but the frame in the visualisation is rotated when I try turning the camera, it doesn't translate to any position when I move the camera.

I am not sure if I use the correct setting for my Asus Xtion Pro and it affects the result though, I have set
camera.bf = 40.0
ThDepth = 50.0
DepthMapFactor = 1000.0
according to some website.

[Thomas00010111]

Sorry for the late reply. I cannot help with your camera settings but what I figured out is that you should disable everything which says "Auto", like Auto White Balance, Auto Exposure, etc.
And the other thing is that it sometimes takes a while till the x,y,z coordinates are updated fluently, and in my case the forward movement (x-direction) worked best.

[LinHuican]

Hi @Thomas00010111 , first of all, thanks your code to publish the camera pose, and then, what is the direction of your camera, forward(x) or down(z)?

[demplo]

Hi @Thomas00010111, Did you manage to have both the pose and the map published on the same reference frame?

I tried your code (as well as mine) but I had no success in having a coherent pose w.r.t. the map (pointcloud generated through mpMap->GetAllMapPoints().

I experience both a mismatch in the axes rotations (e.g. yaw rotation leads to upward/downward rotation) and an offset from the real pose (about 135° roll error in the starting position) which make me think there is a more generic conversion error between the RF used by ROS/Rviz and the one used in ORB SLAM2.

[void-robotics]

Thanks for your code @Thomas00010111. I put your code in ros_stereo.cc and it works! I have the global position of the camera pose using a stereo camera.

[Grifo07]

Hi all!

@Thomas00010111, thank you for your work!! I used it in the ros_mono and it works! I have the pose and quaternion published in ROS.

However, when I try to visualize it in RVIZ I keep getting the error that the quaternion isn't valid. I grabbed the values of the published quaternion, did the norm by hand and got values around 0,97; maybe that is what generates the error since it should be 1 (I think). I assume that's because you use a matrix for the globalRotation_rh instead of a quaternion and, as such, it isn't normalized.

I tried to convert it to a quaternion and publish the transform normalized but I got a quaternion with even lower norm :(. Here's what I did:

static tf::TransformBroadcaster br;
tf::Quaternion q;

tf::Matrix3x3 globalRotation_rh = cameraRotation_rh * rotation270degXZ;
tf::Vector3 globalTranslation_rh = cameraTranslation_rh * rotation270degXZ;

globalRotation_rh.getRotation(q);

tf::Transform transform = tf::Transform(q.normalize(), globalTranslation_rh);

By doing this I get the tf in RVIZ without errors which is odd since the quaternion is even "more invalid" (norm around 0,61). The pose works great but the orientation is really strange when I move the camera.

If anyone could help me that would be great :)
Thank you in advance!

[Thomas00010111]

Hi @grifo, Unfortunately, I do not have the time to work on this project. When I wrote this code I was quite unexperienced in the field of slam, vision, etc. The matrix which you get from the orb slam main module needs to be inverted, I think I used minus signs to achieve this and I am not sure if this correct. If Then try to apply the rotations. Cheers, Thomas 2017-04-13 10:33 GMT-07:00 Grifo <[email protected]>:

…

Hi all! @Thomas00010111 <https://github.com/Thomas00010111>, thank you for your work!! I used it in the ros_mono and it works! I have the pose and quaternion published in ROS. However, when I try to visualize it in RVIZ I keep getting the error that the quaternion isn't valid. I grabbed the values of the published quaternion, did the norm by hand and got values around 0,97; maybe that is what generates the error since it should be 1 (I think). I assume that's because you use a matrix for the globalRotation_rh instead of a quaternion and, as such, it isn't normalized. I tried to convert it to a quaternion and publish the transform normalized but I got a quaternion with even lower norm :(. Here's what I did: `static tf::TransformBroadcaster br; tf::Quaternion q; tf::Matrix3x3 globalRotation_rh = cameraRotation_rh * rotation270degXZ; tf::Vector3 globalTranslation_rh = cameraTranslation_rh * rotation270degXZ; globalRotation_rh.getRotation(q); tf::Transform transform = tf::Transform(q.normalize(), globalTranslation_rh);` By doing this I get the tf in RVIZ without errors which is odd since the quaternion is even "more invalid" (norm around 0,61). The pose works great but the orientation is really strange when I move the camera. If anyone could help me that would be great :) Thank you in advance! — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#102 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/APJF4tBhIpovycGpvD7ofRRejMqCzuzYks5rvlxSgaJpZM4Io9z0> .

[Grifo07]

Hello @Thomas00010111 ,

Thank you for your reply! After thinking a bit I then realized the getRotation(Quaternion q) method should already give me a normalized quaternion. I didn't need to normalize it again, so I change the last code line to:

globalRotation_rh.getRotation(q);

tf::Transform transform = tf::Transform(q, globalTranslation_rh);

And it worked, I now get the transforms in RVIZ.

I'm now having another problem, the transform seems to be initializing around this values:
translation [x: -0.2; y: -0.15; z: 0.05] and rotation [x: -0.5; y: 0.6; z: 0.4; w: 0.5]

It should be initializing around:
translation [x: 0; y: 0; z: 0] and rotation [x: 0; y: 0; z: 0; w: 1]
right?

I get the translation being a bit odd due to the ORB-SLAM algorithm initialization but not the rotation part (quaternion).

I'll try to better analyze your code to see how to get it initializing in the origin (0,0,0)(0,0,0,1) and I'll get back here if I'm able to do so. If anyone have any suggestions I would appreciate it since I'm no algebra expert.

Thank you in advance,
Grifo

[Grifo07]

I got the quaternion to initialize correctly I think :)

Here's what I did, it's a combination of your code with "TFinleyosu" 's answer here

/* global left handed coordinate system */
	static cv::Mat pose_prev = cv::Mat::eye(4,4, CV_32F);
	static cv::Mat world_lh = cv::Mat::eye(4,4, CV_32F);
	// matrix to flip signs of sinus in rotation matrix, not sure why we need to do that
	static const cv::Mat flipSign = (cv::Mat_<float>(4,4) <<   1,-1,-1, 1,
			-1, 1,-1, 1,
			-1,-1, 1, 1,
			1, 1, 1, 1);

	//prev_pose * T = pose
	cv::Mat translation =  (pose * pose_prev.inv()).mul(flipSign);
	world_lh = world_lh * translation;
	pose_prev = pose.clone();

	tf::Matrix3x3 tf3d;
	tf3d.setValue(pose.at<float>(0,0), pose.at<float>(0,1), pose.at<float>(0,2),
			pose.at<float>(1,0), pose.at<float>(1,1), pose.at<float>(1,2),
			pose.at<float>(2,0), pose.at<float>(2,1), pose.at<float>(2,2));

	tf::Vector3 cameraTranslation_rh( world_lh.at<float>(0,3),world_lh.at<float>(1,3), - world_lh.at<float>(2,3) );

	//rotate 270deg about x and 270deg about x to get ENU: x forward, y left, z up
	const tf::Matrix3x3 rotation270degXZ(   0, 1, 0,
			0, 0, 1,
			1, 0, 0);

	static tf::TransformBroadcaster br;

	tf::Matrix3x3 globalRotation_rh = tf3d;
	tf::Vector3 globalTranslation_rh = cameraTranslation_rh * rotation270degXZ;

	tf::Quaternion tfqt;
	globalRotation_rh.getRotation(tfqt);

	double aux = tfqt[0];
	tfqt[0]=-tfqt[2];
	tfqt[2]=tfqt[1];
	tfqt[1]=aux;

	tf::Transform transform;
	transform.setOrigin(globalTranslation_rh);
	transform.setRotation(tfqt);

	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "camera_pose"));

I'm no expert, so it may all be wrong but it works for me ;)

[DalderupMaurice]

Could you address how to use this? @Grifo07
I have edited the ros_rgbd.cc file with your code, rebuild it, ran it, but no topics were published. Could anyone provide the community with some guidelines?

[Grifo07]

Hi @DalderupMaurice,

If you post your code I can help you better maybe. Since yours is RGBD, I can't help you with just a copy paste code.

[DalderupMaurice]

Hi @Grifo07

What I basically did was open the edited files from this pull request which you can find here:
https://github.com/raulmur/ORB_SLAM2/pull/102/files

Replace my original file with the changed file, rebuild the code and that is it.
I have never used c++ so I won't be able to create the publishing of the camera pose etc by my own. I was expecting that the changed file would publish a rostopic when running the algorithm (I'd prefer mono, but rgbd is fine aswell).

Im doing this for my thesis, my goal is to create an autonomous flying quadcopter, but unlike many I did not learn c++ or such complex algorithms/math in my education.

[DalderupMaurice]

Hi @Grifo07
I have managed to get it working. I now have a topic /tf that published the camera pose etc.

The next step for me is having the same result while using Monoculaire.

[Grifo07]

Hey @DalderupMaurice !

Sorry for the late reply! To use the monocular you can just copy paste the code I put in a previous answer. It publishes in the /tf topic the position and quaternion.

I'm able to use it to navigate a quadcopter. You just have to modify it a bit to incorporate scale in the monocular case. I'm still trying to do this.

However these guys: https://github.com/idsc-frazzoli/ardrone_testbed and https://www.youtube.com/watch?v=Bz3eOuCs-pI did a great job and released their code. If I had seen this at first I would try to branch my code from there. Maybe you have better luck ;)

[Hartvik90]

Grifo07, your reply in the comment works perfectly, spent hours to get the original code to work. Thank you soo much, please make a pull request to Thomas00010111

[Grifo07]

Hello there,

Thanks @Hartvik90, however, ignore that code I wrote :P The reason is because I still didn't know much about quaternions at that time. And basically what mine does is using the same technique as Thomas00010111's but initialized in the origin. We still have a bit of drift error, which means that after a while the position in ROS will be completely different from the one ORB-SLAM2 returns.

So I did something different now:

tf::Quaternion hamiltonProduct(tf::Quaternion a, tf::Quaternion b) {

	tf::Quaternion c;

		c[0] = (a[0]*b[0]) - (a[1]*b[1]) - (a[2]*b[2]) - (a[3]*b[3]);
		c[1] = (a[0]*b[1]) + (a[1]*b[0]) + (a[2]*b[3]) - (a[3]*b[2]);
		c[2] = (a[0]*b[2]) - (a[1]*b[3]) + (a[2]*b[0]) + (a[3]*b[1]);
		c[3] = (a[0]*b[3]) + (a[1]*b[2]) - (a[2]*b[1]) + (a[3]*b[0]);

	return c;
}

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 = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());

	if (pose.empty())
		return;

	//Quaternion
	tf::Matrix3x3 tf3d;
	tf3d.setValue(pose.at<float>(0,0), pose.at<float>(0,1), pose.at<float>(0,2),
			pose.at<float>(1,0), pose.at<float>(1,1), pose.at<float>(1,2),
			pose.at<float>(2,0), pose.at<float>(2,1), pose.at<float>(2,2));

	tf::Quaternion tfqt;
	tf3d.getRotation(tfqt);
	double aux = tfqt[0];
		tfqt[0]=-tfqt[2];
		tfqt[2]=tfqt[1];
		tfqt[1]=aux;



	//Translation for camera
	tf::Vector3 origin;
	origin.setValue(pose.at<float>(0,3),pose.at<float>(1,3),pose.at<float>(2,3));
	//rotate 270deg about x and 270deg about x to get ENU: x forward, y left, z up
	const tf::Matrix3x3 rotation270degXZ(   0, 1, 0,
											0, 0, 1,
											-1, 0, 0);

	tf::Vector3 translationForCamera = origin * rotation270degXZ;

	//Hamilton (Translation for world)
	tf::Quaternion quaternionForHamilton(tfqt[3], tfqt[0], tfqt[1], tfqt[2]);
	tf::Quaternion secondQuaternionForHamilton(tfqt[3], -tfqt[0], -tfqt[1], -tfqt[2]);
	tf::Quaternion translationHamilton(0, translationForCamera[0], translationForCamera[1], translationForCamera[2]);

	tf::Quaternion translationStepQuat;
	translationStepQuat = hamiltonProduct(hamiltonProduct(quaternionForHamilton, translationHamilton), secondQuaternionForHamilton);

	tf::Vector3 translation(translationStepQuat[1], translationStepQuat[2], translationStepQuat[3]);

	//Scaling
	if(m_numScales > 0) {
		translation = m_scale * translation;
	}
	//Set world
	m_currentQ = tfqt;
	m_currentT = translation;
	translation = translation - m_worldT;
	tfqt = tfqt * m_worldQ.inverse();

	//Creates transform and populates it with translation and quaternion
	tf::Transform transformCurrent;
	transformCurrent.setOrigin(translation);
	transformCurrent.setRotation(tfqt);
	//Publishes transform
	static tf::TransformBroadcaster br;
	br.sendTransform(tf::StampedTransform(transformCurrent, ros::Time::now(), "world", "camera_pose"));

}

Explanation: https://en.wikipedia.org/wiki/Quaternion#Hamilton_product

In short: Basically I use the hamilton product to orient the translation vector, meaning I go from local coordinates to world coordinates. What Thomas00010111 did and I copied, was to try to see what was the travel distance and orientation from the last position and add it to an accumulator (the pose). As you can guess, each calculation has an error which will increase with each cycle. This way we eliminate this :)

Oh and ignore the Scalling if you don't use it. I use it to change the coordinates to actual metres.

Sorry for the long post,
Hope this helps,

[hmchung]

Hi @Grifo07, thank you very much for sharing. I am still trying to use your code.
However, at the first glance, I think you might have double-paste your code :)

[hmchung]

HI @Grifo07,
I manage to compile and run the lower part of your shared code.
However, I had to comment out the scaling part. Can you please also explain how you use this?
Thank you very much for sharing. Here is what works for me.
```
cv::Mat pose = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
if (pose.empty()) return;
tf::Matrix3x3 tf3d;
tf3d.setValue(pose.at(0,0), pose.at(0,1), pose.at(0,2),
pose.at(1,0), pose.at(1,1), pose.at(1,2),
pose.at(2,0), pose.at(2,1), pose.at(2,2));

tf::Quaternion tfqt;
tf3d.getRotation(tfqt);
double aux = tfqt[0];
    tfqt[0]=-tfqt[2];
    tfqt[2]=tfqt[1];
    tfqt[1]=aux;

//Translation for camera
tf::Vector3 origin;
origin.setValue(pose.at<float>(0,3),pose.at<float>(1,3),pose.at<float>(2,3));
//rotate 270deg about x and 270deg about x to get ENU: x forward, y left, z up
const tf::Matrix3x3 rotation270degXZ(   0, 1, 0,
                                        0, 0, 1,
                                        -1, 0, 0);

tf::Vector3 translationForCamera = origin * rotation270degXZ;

//Hamilton (Translation for world)
tf::Quaternion quaternionForHamilton(tfqt[3], tfqt[0], tfqt[1], tfqt[2]);
tf::Quaternion secondQuaternionForHamilton(tfqt[3], -tfqt[0], -tfqt[1], -tfqt[2]);
tf::Quaternion translationHamilton(0, translationForCamera[0], translationForCamera[1], translationForCamera[2]);

tf::Quaternion translationStepQuat;
translationStepQuat = hamiltonProduct(hamiltonProduct(quaternionForHamilton, translationHamilton), secondQuaternionForHamilton);

tf::Vector3 translation(translationStepQuat[1], translationStepQuat[2], translationStepQuat[3]);

// //Scaling
// if(m_numScales > 0) {
//     translation = m_scale * translation;
// }
// //Set world
// m_currentQ = tfqt;
// m_currentT = translation;
// translation = translation - m_worldT;
// tfqt = tfqt * m_worldQ.inverse();

//Creates transform and populates it with translation and quaternion
tf::Transform transformCurrent;
transformCurrent.setOrigin(translation);
transformCurrent.setRotation(tfqt);

br->sendTransform(tf::StampedTransform(transformCurrent, ros::Time::now(), "world", "orb_slam"));

[LiangLiang128]

I have encountered the same problem with @hmchung, and I try to define these variable.However,It dose not work.Would you please help us with this problem? @Grifo07, Thanks a lot:)

[hesamira]

Dear @Grifo07,

Can you share your ros_mono.cc? I need to publish the pose so I should add a topic to do that.
I have tested different codes, but They couldn't publish the pose.
Please, guide me.
Thanks.

[dishank-b]

@Grifo07 , @Thomas00010111 @hmchung .
Thanks for sharing your code for finding the Pose. But I would like to know as ORB SLAM is a monocular approach, it must not be able to get the exact scale. Then how is it possible to find the pose in world coordinate without knowing the actual scale.

[Grifo07]

Hi there!

@hmchung and @LiangLiang128 , you are right, I accidentally double-pasted my code. I edited my comment to reflect my actual code now. However, the correct part was the top part :/
The scaling part is used to "scale the world". So imagine the ORB-SLAM gives me 1 unit to the right when I actually moved 3 meters. Basically my scale is 3. So, during my application, I come up with the scale and multiply it by the output of the ORB-SLAM getting my position in meters.

@hesamira, this comment reflects my ros_mono.cc. The part you should look better for is the tf::TransformBroadcaster structure.

@dishank-b, ORB-SLAM gets its position by comparing its current features with the features got in the last frame of the video stream. So, it's basically just a relative position, relative to the first position it ever tracked. So the scale is defined by the speed you move the camera when you initialize the algorithm. I assume it assumes the scale as one. So, if you move the camera lets say 2 meters before it gets the first relation between the first frames, it will assume you moved 1 unit and your scale will be 2. But I guess there is no way of knowing your scale without actually measuring it. In my application my drone has a IR sensor to measure distance, and by moving it up and down, I can measure the actual distance it traveled vs the ORB-SLAM distance and get a scale.

Hope I helped,
Sorry for the late response,
Cheers,

[dishank-b]

Thanks for the reply. It is a real help.

…

On Mon, Aug 27, 2018 at 8:02 PM Grifo ***@***.***> wrote: Hi there! @hmchung <https://github.com/hmchung> and @LiangLiang128 <https://github.com/LiangLiang128> , you are right, I accidentally double-pasted my code. I edited my comment to reflect my actual code now. However, the correct part was the top part :/ The scaling part is used to "scale the world". So imagine the ORB-SLAM gives me 1 unit to the right when I actually moved 3 meters. Basically my scale is 3. So, during my application, I come up with the scale and multiply it by the output of the ORB-SLAM getting my position in meters. @hesamira <https://github.com/hesamira>, this comment <#102 (comment)> reflects my ros_mono.cc. The part you should look better for is the tf::TransformBroadcaster <http://docs.ros.org/melodic/api/tf/html/c++/classtf_1_1TransformBroadcaster.html> structure. @dishank-b <https://github.com/dishank-b>, ORB-SLAM gets its position by comparing its current features with the features got in the last frame of the video stream. So, it's basically just a relative position, relative to the first position it ever tracked. So the scale is defined by the speed you move the camera when you initialize the algorithm. I assume it assumes the scale as one. So, if you move the camera lets say 2 meters before it gets the first relation between the first frames, it will assume you moved 1 unit and your scale will be 2. But I guess there is no way of knowing your scale without actually measuring it. In my application my drone has a IR sensor to measure distance, and by moving it up and down, I can measure the actual distance it traveled vs the ORB-SLAM distance and get a scale. Hope I helped, Sorry for the late response, Cheers, — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#102 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AQ6RJdBw53G9cR6vDBTZn9lUalvEQo8Vks5uVALhgaJpZM4Io9z0> .

-- Regards, *Dishank Bansal* Junior Year Undergraduate Student IIT Kharagpur Ph: +91 9800394333 Website <https://dishank-b.github.io/> | Git Hub <https://github.com/dishank-b> Computer Vision Team Member Autonomous Ground Vehicle(AGV) Research Group <http://agv-iit-kgp.github.io> IIT Kharagpur

[DodsGTH94]

Hi @Grifo07 and @Thomas00010111.
I´m a student and I´m working now on Orb_Slam2 and all what it´s related to it.
I would like to point out a problem I´m facing right now: I´m doing a SLAM of a room, with two or three positions marked. I measured the distance between them and the starting point.
First question: does the system initialize always in (0;0;0)?

Second question: I read about the drift the program might have, but if I move to one point to another 1 meter far away it tells me only 0,9, which I guess are centimeters and not meters.

Third question: To know the camera position, I have to "rostopic echo" the topic /tf, right?

Thank you for your help!!

[Grifo07]

Hi @DodsGTH94,

Answering you questions:
First one:
It does. Basically, when it finally initializes (green lines disappear and you start to see the green dots), it assumes the first position of the initialization (beginning of the green lines) as the (0,0,0) position.

Second question:
I kind of didn't get the question but I'll try to answer it anyway. ORB_SLAM does havce drift but tries to compensate it by fixing the geometry when finding an already visited position, that's why the SLAM. However, in the monocular approach, you don't have units. So, the 0.9 that you are getting is not in meters or centimeters. It is in an arbitrary unit that depends on the speed you initialize the algorithm with (the length of the green lines when they turn into dots). To convert this unit to meters I have in my code the variable m_scale which is the relation between meters and this arbitrary unit. I get this value by using an IR sensor on the drone.

Third question:
Only if you want to see the position on the terminal. In your program you should subscribe to the /tf topic and get the pose.

I answered your questions only with what I know. I'm not saying everything I said is true, but it's what I believe to be true, from my experience with this algorithm. I advice you to do your own research and experiences also.

Best of lucks,
Hope it helps,
Grifo

[DodsGTH94]

Thank you @Grifo07 .
First of all I never see green lines, but only green dots in the rgb image. But maybe it´s just a matter of visualization.
Second thing, I´m using a rgbd camera, so I used the ros_rgbd.cc file uploaded by @Thomas00010111 .
And here #102 (comment) he tells that the position is expressed in cm. Is it right?
Anyway I will try to adapt your code to my rgbd camera in order to have those kind of conversion.

Thank you all!!

[Grifo07]

Hi @DodsGTH94,
If you're using rgbd you won't see the green lines. The green lines appear when you're initializing the monocular algorithm.
Since I never used an rgbd camera with this, I do not know how does it behave with units :/
However, my code should work for you, just take the //Scaling part out since that is intended for the monocular algorithm.

Cheers!

[jnre]

@Grifo07 thanks alot! got it to work for monocular, and published the transform. However i did it without the scale and world, (commented out, same as what @hmchung and @LiangLiang128 are asking) .im wondering how do u set the m_worldQ and m_worldT and what are these variables? In what situation does m_numScales fall below 1?

[Grifo07]

So, I'll try to explain the scale part (ONLY FOR MONOCULAR):

Basically I initialize my flight (I'm using a quadcopter), it initializes the ORB_SLAM2 and I start to get a pose. However I get it in unscaled units, meaning I know the drone is let's say 2 units in the air but I have no idea how does that translate to meters.
So, I move the drone up and downwards measuring the actual traveled meters with an IR sensor and measuring the traveled units in the ORB_SLAM algorithm. From then it's simple math, let's say I move the drone once and it gives me 0.66 ORB_SLAM units and 2 meters. I then know the scale is m_scale= 2/0.66 = 3. That means I can multiply the the pose returned by the ORB_SLAM by 3 and get the pose in meters all the time.
This is specially useful when you have to tune a PID for such a system. Since the ORB_SLAM units are never the same, you can not find suitable PID parameters that work every time. With the scale you can tune your PID for meters and have it working all the time :)

Now imagine we move the drone again and get another value for the scale, let's say 3.2. I then do numScales++ and m_scale = (m_scale + 3.2) / numScales, improving my overall scale with the average of all the measured scales. I did this because my drone was very tiny and unstable, this way I could improve the scale with each up and down movement.

Hope it helps,
Grifo

[jnre]

@Grifo07 thanks for explaining the scale for tuning purposes. What about the m_worldT and m_worldQ? were u trying to assume a different world frame instead of (0,0,0) and mathematically why is an inverse applied? Anyway, am really grateful for the quaternion transformation! Maybe it would be good if you could show what is the inital frame of global and camera positions

[Grifo07]

I can't really recall that part since it was a while ago, but it served the purpose of setting the current position of the drone as the world reference. Basically I had a service that I called whenever I wanted and that service would just do:

m_worldQ = m_currentQ;
m_worldT = m_currentT;

I don't remember much about the math behind it and I really don't have much time to dig into it.

Maybe it would be good if you could show what is the inital frame of global and camera positions

Can't really understand this question. They both start at (0,0,0).

[DodsGTH94]

Hi @DalderupMaurice ,
I have a question for you. Did you manage to get the code running in a proper way? I mean, the pose was accurate? Did you do some test?
Thank you

[SameerSangle]

Thanks for your code @Thomas00010111. I put your code in ros_stereo.cc and it works! I have the global position of the camera pose using a stereo camera.

can u tell me what changes u had to make to apply this to stereo cameras?

[DigitalGabriele]

To anyone who needs it: this is the working code for Mono https://pastebin.com/tiPpsGLW

@Grifo07 , does it mean that the camera_pose is where the SLAM THINKS it is?
How do I interpret the /camera_pose tf? I see Translation and Rotation matrices, does it mean its at X, Y, Z (taken from translation) in some magical units?

Even if I run rostopic echo /tf I get the following:

frame_id: world
child_frame_id: camera_pose
Translation: x: 0.01, y: 0.2, z:0.4
Rotation: x: 0.11, y: -0.3, z: 0.06, w: 0.96

Does it mean that the camera_pose is relative to the world at Translation T and Rotation R? So in order to get the actual position I need to do some inverse?
I dont mind getting the camera position (x,y,z in world coordinates) in the simulator's units.

@DodsGTH94 , I tried looking at the above mentioned translations and rotations and compare them to ground truth. It seems like that that the transform's stamp does not follow the other transforms stamp sequence and the pose does not match any of the ground truth data.

All the other transforms follow the right sequence (around 1305031251) in rgbd_dataset_freiburg1_rpy.bag from TUM, while the /world /camera_pose shows transforms at around 1551195194 secs. Therefore I dont know how to even compare it.

[Grifo07]

@DigitalGabriele, it was a while ago, so I may be mistaken, but I believe the camera_pose, after the hamilton product, represents the position of the camera with the initial position as reference. However they are still in "magical" units indeed. For me to get real units (metres), I use my //Scaling piece of code to scale the magical units to metres.

[DigitalGabriele]

@Grifo07, thank you so much for answering! What about the other question:
I tried looking at the above mentioned translations and rotations and compare them to ground truth. It seems like that that the transform's stamp does not follow the other transforms stamp sequence and the pose does not match any of the ground truth data.

Furthermore, on the example I ran (its almost 30sec long), I only got around 300 /camera_pose tfs, while for other tfs, like /world /kinect, it was thousands.

All the other transforms follow the right sequence (around 1305031251) in rgbd_dataset_freiburg1_rpy.bag from TUM, while the /world /camera_pose shows transforms at around 1551195194 secs. Therefore I dont know how to even compare it.

One more question: is it possible to use EKF on top of the camera_pose to enhance the ORB-SLAM's accuracy or do you think that should be done earlier in the pipe?