Saving pose data from multiple T265s, drift error.

[antithing]

---

Required Info
Camera Model	{ T265 }
Firmware Version	2.19.2
Operating System & Version	{Win 10}
Platform	PC
SDK Version	2.19.2
Language	C++

Issue Description

I have an application where I am streaming pose data from three T265 cameras.
Once I graph the data that I save, I see huge amounts of drift when i move the camera cluster.

Am I storing the data correctly here?

int main(int argc, char * argv[]) try
{

	bool saver = false;
	rs2::context                ctx;            // Create librealsense context for managing devices
	rs2::colorizer              colorizer;      // Utility class to convert depth data RGB colorspace

	std::vector<rs2::pipeline>  pipelines;
	std::vector<rs2::pipeline_profile> pipe_profiles;
	std::vector<std::string> serials;
	// Start a streaming pipe per each connected device
	for (auto&& dev : ctx.query_devices())
	{
		rs2::pipeline pipe(ctx);
		rs2::config cfg;
		cfg.enable_device(dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
		
		cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
		cfg.enable_stream(RS2_STREAM_FISHEYE, 1);
		cfg.enable_stream(RS2_STREAM_FISHEYE, 2);
		rs2::pipeline_profile pp = pipe.start(cfg);
		pipelines.emplace_back(pipe);
		//pipe_profiles.emplace_back(pp);

		serials.emplace_back(dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
	}


	for (auto pipe : pipe_profiles)
	{
			
	int saveTime = 0;
	int ffnum = 0;
	std::string nameL;
	std::string nameR;
	std::vector<cv::Mat> allframes;
	std::vector<std::string> allIds;
	int cameraNumber = 0;

	std::ofstream file1;
	file1.open("Cam0.csv");
	std::ofstream file2;
	file2.open("Cam1.csv");
	std::ofstream file3;
	file3.open("Cam2.csv");


//	std::cout << serials[0] << std::endl;
	// Capture 30 frames to give autoexposure, etc. a chance to settle
	for (auto &&pipe : pipelines)
	{
		for (auto i = 0; i < 30; ++i) pipe.wait_for_frames();
	}

	// Main app loop
	while (true)
	{
		// Collect the new frames from all the connected devices
		std::vector<rs2::frame> new_frames;
			

		for (auto &&pipe : pipelines)
		{		
			//905312111346 is right side from top down
			//905312111190 is middle
			//845412111384 is left

			std::string serialNum = pipe.get_active_profile().get_device().get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);

			if (serialNum == "905312111346")
			{
				//right hand cam (from top down)
				cameraNumber = 0;
			}
			if (serialNum == "905312111190")
			{
				//middle
				cameraNumber = 2;
			}
			if (serialNum == "845412111384")
			{
				//left
				cameraNumber = 4;
			}

			rs2::frameset fs;
			if (pipe.poll_for_frames(&fs))
			{
				int id = fs.get_profile().unique_id();
				
				// Get a frame from the pose stream
				rs2::pose_frame pose_frame = fs.get_pose_frame();
				auto pose_data = pose_frame.get_pose_data();

				auto now = std::chrono::system_clock::now().time_since_epoch();
				double now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now).count();
				double pose_time_ms = fs.get_timestamp();
				float dt_s = static_cast<float>(std::max(0., (now_ms - pose_time_ms) / 1000.));
				rs2_pose predicted_pose = predict_pose2(pose_data, dt_s);

				//save pose data
				if (cameraNumber == 2)
				{
					file1 << "0, " + std::to_string(pose_data.translation.x) + "," + 
						std::to_string(pose_data.translation.y) + "," +
						std::to_string(pose_data.translation.z) + "," +
						std::to_string(pose_data.rotation.x) + "," +
						std::to_string(pose_data.rotation.y) + "," +
						std::to_string(pose_data.rotation.z) + "," +
						std::to_string(pose_data.rotation.w) + "," +
						std::to_string(pose_data.tracker_confidence) + ".\n";

				}
				if (cameraNumber == 0)
				{
					file2 << "1, " + std::to_string(pose_data.translation.x) + "," +
						std::to_string(pose_data.translation.y) + "," +
						std::to_string(pose_data.translation.z) + "," +
						std::to_string(pose_data.rotation.x) + "," +
						std::to_string(pose_data.rotation.y) + "," +
						std::to_string(pose_data.rotation.z) + "," +
						std::to_string(pose_data.rotation.w) + "," +
						std::to_string(pose_data.tracker_confidence) + ".\n";

				}
				if (cameraNumber == 4)
				{
					file3 << "2, " + std::to_string(pose_data.translation.x) + "," +
						std::to_string(pose_data.translation.y) + "," +
						std::to_string(pose_data.translation.z) + "," +
						std::to_string(pose_data.rotation.x) + "," +
						std::to_string(pose_data.rotation.y) + "," +
						std::to_string(pose_data.rotation.z) + "," +
						std::to_string(pose_data.rotation.w) + "," +
						std::to_string(pose_data.tracker_confidence) + ".\n";

				}

					rs2::video_frame fisheye_frameL = fs.get_fisheye_frame(1);
					rs2::frame frameLeft = colorizer.process(fisheye_frameL);
					rs2::video_frame fisheye_frameR = fs.get_fisheye_frame(2);
					rs2::frame frameRight = colorizer.process(fisheye_frameR);

					nameL = std::to_string(cameraNumber + 1);
					nameR = std::to_string(cameraNumber + 0);
									   					
					cv::Mat frameMatL = frame2Mat2(frameLeft);
					cv::imshow(nameL, frameMatL);
					cv::Mat frameMatR = frame2Mat2(frameRight);
					cv::imshow(nameR, frameMatR);
	   				cv::waitKey(1);
			}
		}
	}

	return EXIT_SUCCESS;
}

Additionally, which is more robust / accurate:

rs2_pose predicted_pose = predict_pose2(pose_data, dt_s);

or simply:

auto pose_data = pose_frame.get_pose_data();

Thank you.

[RealSenseCustomerSupport]

---

Hi @antithing

I don't see a problem with the saving of pose data. Although you mention drift in the same statement.

How are you handling the cameras? Do you have them all affixed to the same jig?
Are you taking into account the center of tracking and extrinsics between the 3 cameras?
When you say drift, is it drift between the 3 cameras or are you using another tracking solution that you are comparing the 3 camera's data to?
Using pose prediction is an option as you've stated. Have you taken a look or reviewed the sample we have for pose prediction?

Also our newer version has updates to the relocalization feature.

[antithing]

Hi, thank you for getting back to me. it is drift between the cameras. Which I could be seeing due to my extrinsics being slightly off. I am trying to improve these with a bundle adjustment step, but i am struggling to stereo triangulate points with the t265. I have been fighting this for a week, and cannot seem to get it right. I am using opencv, feature matching, and using the onboard intrinsics to calculate projection matrices, then triangulating points.
No matter what i try, the 3d points are very wrong.

Is there any example code around for doing this with a t265? Any help would be much appreciated!

Thanks again for your time.

[RealSenseCustomerSupport]

---

Maybe this can help: #3951
But I think you may have another Github issue pending with this same PR that could help you: #3880

[RealSenseCustomerSupport]

---

Will you be needing further assistance with this?

[antithing]

Hi! This is solved. thank you!