You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
317 lines
13 KiB
317 lines
13 KiB
// License: Apache 2.0. See LICENSE file in root directory.
|
|
// Copyright(c) 2021 Intel Corporation. All Rights Reserved.
|
|
|
|
//#test:device D400*
|
|
|
|
#include <rsutils/easylogging/easyloggingpp.h>
|
|
|
|
#include "../../catch.h"
|
|
|
|
|
|
#include <librealsense2/rs.hpp>
|
|
#include <librealsense2/rsutil.h>
|
|
|
|
using namespace rs2;
|
|
|
|
// structure of a matrix 4 X 4, representing rotation and translation as following:
|
|
// pos_and_rot[i][j] is
|
|
// _ _
|
|
// | | |
|
|
// | rotation | translation |
|
|
// | (3x3) | (3x1) |
|
|
// | _________ |____________ |
|
|
// | 0 | 1 |
|
|
// |_ (1x3) | (1x1) _|
|
|
//
|
|
struct position_and_rotation {
|
|
double pos_and_rot[4][4];
|
|
// rotation tolerance - units are in cosinus of radians
|
|
const double rotation_tolerance = 0.00001;
|
|
// translation tolerance - units are in meters
|
|
const double translation_tolerance = 0.0001; // 0.1mm
|
|
|
|
position_and_rotation operator* (const position_and_rotation& other)
|
|
{
|
|
position_and_rotation product;
|
|
for (int i = 0; i < 4; i++) {
|
|
for (int j = 0; j < 4; j++) {
|
|
product.pos_and_rot[i][j] = 0;
|
|
for (int k = 0; k < 4; k++)
|
|
product.pos_and_rot[i][j] += pos_and_rot[i][k] * other.pos_and_rot[k][j];
|
|
}
|
|
}
|
|
return product;
|
|
}
|
|
|
|
bool equals(const position_and_rotation& other)
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
for (int j = 0; j < 4; ++j)
|
|
{
|
|
double tolerance = rotation_tolerance;
|
|
if (j == 3) tolerance = translation_tolerance;
|
|
|
|
if (fabs(pos_and_rot[i][j] - other.pos_and_rot[i][j]) > tolerance)
|
|
{
|
|
std::cout << "i,j = " << i << "," << j << ", pos_and_rot[i][j] = " << pos_and_rot[i][j] << std::endl;
|
|
std::cout << "required value = " << other.pos_and_rot[i][j] << std::endl;
|
|
std::cout << "tolerance = " << tolerance << std::endl;
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
bool is_identity()
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
for (int j = 0; j < 4; ++j)
|
|
{
|
|
double target = 0.0;
|
|
if (i == j) target = 1.0;
|
|
|
|
double tolerance = rotation_tolerance;
|
|
if (j == 3) tolerance = translation_tolerance;
|
|
|
|
if (fabs(pos_and_rot[i][j] - target) > tolerance)
|
|
{
|
|
std::cout << "i,j = " << i << "," << j << ", pos_and_rot[i][j] = " << pos_and_rot[i][j] << std::endl;
|
|
std::cout << "required value = " << target << std::endl;
|
|
std::cout << "tolerance = " << tolerance << std::endl;
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
};
|
|
|
|
|
|
// This method takes in consideration that the totation array is given in the order of a column major 3x3 matrix
|
|
position_and_rotation matrix_4_by_4_from_translation_and_rotation(const float* position, const float* rotation)
|
|
{
|
|
position_and_rotation pos_rot;
|
|
pos_rot.pos_and_rot[0][0] = static_cast<double>(rotation[0]);
|
|
pos_rot.pos_and_rot[1][0] = static_cast<double>(rotation[1]);
|
|
pos_rot.pos_and_rot[2][0] = static_cast<double>(rotation[2]);
|
|
|
|
pos_rot.pos_and_rot[0][1] = static_cast<double>(rotation[3]);
|
|
pos_rot.pos_and_rot[1][1] = static_cast<double>(rotation[4]);
|
|
pos_rot.pos_and_rot[2][1] = static_cast<double>(rotation[5]);
|
|
|
|
pos_rot.pos_and_rot[0][2] = static_cast<double>(rotation[6]);
|
|
pos_rot.pos_and_rot[1][2] = static_cast<double>(rotation[7]);
|
|
pos_rot.pos_and_rot[2][2] = static_cast<double>(rotation[8]);
|
|
|
|
pos_rot.pos_and_rot[3][0] = 0.0;
|
|
pos_rot.pos_and_rot[3][1] = 0.0;
|
|
pos_rot.pos_and_rot[3][2] = 0.0;
|
|
|
|
pos_rot.pos_and_rot[0][3] = static_cast<double>(position[0]);
|
|
pos_rot.pos_and_rot[1][3] = static_cast<double>(position[1]);
|
|
pos_rot.pos_and_rot[2][3] = static_cast<double>(position[2]);
|
|
|
|
pos_rot.pos_and_rot[3][3] = 1.0;
|
|
|
|
return pos_rot;
|
|
}
|
|
|
|
std::string to_string(const rs2_extrinsics& extrinsics)
|
|
{
|
|
std::stringstream ss;
|
|
ss << extrinsics.rotation[0] << " " << extrinsics.rotation[3] << " " << extrinsics.rotation[6] << std::endl;
|
|
ss << extrinsics.rotation[1] << " " << extrinsics.rotation[4] << " " << extrinsics.rotation[7] << std::endl;
|
|
ss << extrinsics.rotation[2] << " " << extrinsics.rotation[5] << " " << extrinsics.rotation[8] << std::endl << std::endl;
|
|
ss << extrinsics.translation[0] << " " << extrinsics.translation[1] << " " << extrinsics.translation[2] << std::endl;
|
|
|
|
return ss.str();
|
|
}
|
|
|
|
std::string to_string( const position_and_rotation & mat )
|
|
{
|
|
std::stringstream ss;
|
|
ss << mat.pos_and_rot[0][0] << " " << mat.pos_and_rot[0][1] << " " << mat.pos_and_rot[0][2] << " " << mat.pos_and_rot[0][3] << std::endl;
|
|
ss << mat.pos_and_rot[1][0] << " " << mat.pos_and_rot[1][1] << " " << mat.pos_and_rot[1][2] << " " << mat.pos_and_rot[1][3] << std::endl;
|
|
ss << mat.pos_and_rot[2][0] << " " << mat.pos_and_rot[2][1] << " " << mat.pos_and_rot[2][2] << " " << mat.pos_and_rot[2][3] << std::endl;
|
|
ss << mat.pos_and_rot[3][0] << " " << mat.pos_and_rot[3][1] << " " << mat.pos_and_rot[3][2] << " " << mat.pos_and_rot[3][3] << std::endl;
|
|
|
|
return ss.str();
|
|
}
|
|
|
|
const double rotation_diagonal_min = 0.95;
|
|
const double rotation_diagonal_max = 1.0;
|
|
bool check_calibration_criteria(const rs2_extrinsics& extrinsics)
|
|
{
|
|
if (extrinsics.rotation[0] > rotation_diagonal_min && extrinsics.rotation[0] <= rotation_diagonal_max &&
|
|
extrinsics.rotation[4] > rotation_diagonal_min && extrinsics.rotation[4] <= rotation_diagonal_max &&
|
|
extrinsics.rotation[8] > rotation_diagonal_min && extrinsics.rotation[8] <= rotation_diagonal_max)
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
bool is_rgb_calibrated(const std::vector<rs2::stream_profile>& profiles)
|
|
{
|
|
bool res = false;
|
|
std::vector<rs2::stream_profile> depth_and_color_profiles;
|
|
bool depth_profile_added = false;
|
|
bool color_profile_added = false;
|
|
|
|
for (auto&& p : profiles)
|
|
{
|
|
// getting depth profile
|
|
if (!depth_profile_added && p.stream_type() == RS2_STREAM_DEPTH)
|
|
{
|
|
depth_and_color_profiles.push_back(p);
|
|
depth_profile_added = true;
|
|
}
|
|
// getting color profile
|
|
if (!color_profile_added && p.stream_type() == RS2_STREAM_COLOR)
|
|
{
|
|
depth_and_color_profiles.push_back(p);
|
|
color_profile_added = true;
|
|
}
|
|
if (depth_profile_added && color_profile_added)
|
|
break;
|
|
}
|
|
|
|
if (depth_and_color_profiles.size() == 2)
|
|
{
|
|
rs2_extrinsics depth_color_extrinsics = depth_and_color_profiles[0].get_extrinsics_to(depth_and_color_profiles[1]);
|
|
res = check_calibration_criteria(depth_color_extrinsics);
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
// checking the extrinsics graph
|
|
// steps are:
|
|
// 1. get all the profiles
|
|
// 2. for each pair of these profiles:
|
|
// check that:
|
|
// point P(x,y,z,head,pitch,roll) in profile A transformed to profile B coordinates, and then transformed back to profile A coordinates == same point P
|
|
TEST_CASE("Extrinsics graph - matrices 4x4", "[live]")
|
|
{
|
|
rs2::context ctx;
|
|
auto list = ctx.query_devices();
|
|
|
|
for (auto&& device : list)
|
|
{
|
|
std::cout << "device: " << device.get_info(RS2_CAMERA_INFO_NAME) << std::endl;
|
|
auto sensors = device.query_sensors();
|
|
|
|
// getting stream profiles from all sensors
|
|
std::vector<rs2::stream_profile> profiles;
|
|
for (auto&& sensor : sensors)
|
|
{
|
|
std::vector<rs2::stream_profile> stream_profiles = sensor.get_stream_profiles();
|
|
for (auto&& profile : stream_profiles)
|
|
{
|
|
profiles.push_back(profile);
|
|
}
|
|
}
|
|
|
|
std::vector<rs2_stream> streams_to_ignore = { RS2_STREAM_ACCEL, RS2_STREAM_GYRO };
|
|
bool rgb_calibrated = is_rgb_calibrated(profiles);
|
|
if (!rgb_calibrated)
|
|
{
|
|
std::cout << "RGB is not calibrated on this device" << std::endl;
|
|
streams_to_ignore.push_back(RS2_STREAM_COLOR);
|
|
}
|
|
std::vector<rs2::stream_profile> relevant_profiles;
|
|
|
|
for (auto&& i : profiles)
|
|
{
|
|
auto stream_type = i.stream_type();
|
|
auto it = find(streams_to_ignore.begin(), streams_to_ignore.end(), stream_type);
|
|
if (it == streams_to_ignore.end())
|
|
relevant_profiles.push_back(i);
|
|
}
|
|
|
|
float start_point[3] = { 1.f, 2.f, 3.f };
|
|
|
|
for (int i = 0; i < relevant_profiles.size() - 2; ++i)
|
|
{
|
|
for (int j = i + 1; j < relevant_profiles.size() - 1; ++j)
|
|
{
|
|
rs2_extrinsics extr_i_to_j = relevant_profiles[i].get_extrinsics_to(relevant_profiles[j]);
|
|
rs2_extrinsics extr_j_to_i = relevant_profiles[j].get_extrinsics_to(relevant_profiles[i]);
|
|
|
|
position_and_rotation pr_i_to_j = matrix_4_by_4_from_translation_and_rotation(extr_i_to_j.translation, extr_i_to_j.rotation);
|
|
position_and_rotation pr_j_to_i = matrix_4_by_4_from_translation_and_rotation(extr_j_to_i.translation, extr_j_to_i.rotation);
|
|
|
|
position_and_rotation product = pr_i_to_j * pr_j_to_i;
|
|
// checking that product of extrinsics from i to j with extrinsiscs from j to i is identity matrix
|
|
if (!product.is_identity())
|
|
{
|
|
std::cout << "i : stream type: " << relevant_profiles[i].stream_type()
|
|
<< ", format: " << relevant_profiles[i].format()
|
|
<< ", fps: " << relevant_profiles[i].fps()
|
|
<< ", stream index: " << relevant_profiles[i].stream_index() << std::endl;
|
|
|
|
std::cout << "j : stream type: " << relevant_profiles[j].stream_type()
|
|
<< ", format: " << relevant_profiles[j].format()
|
|
<< ", fps: " << relevant_profiles[j].fps()
|
|
<< ", stream index: " << relevant_profiles[j].stream_index() << std::endl;
|
|
|
|
std::cout << "extr_i_to_j : " << std::endl;
|
|
std::cout << to_string(extr_i_to_j) << std::endl;
|
|
std::cout << "extr_j_to_i : " << std::endl;
|
|
std::cout << to_string(extr_j_to_i) << std::endl;
|
|
std::cout << std::endl;
|
|
|
|
std::cout << to_string( product ) << std::endl;
|
|
}
|
|
REQUIRE(product.is_identity());
|
|
|
|
// checking with API rs2_transform_point_to_point
|
|
float transformed_point[3];
|
|
rs2_transform_point_to_point(transformed_point, &extr_i_to_j, start_point);
|
|
float end_point[3];
|
|
rs2_transform_point_to_point(end_point, &extr_j_to_i, transformed_point);
|
|
bool res = true;
|
|
for (int t = 0; t < 3; ++t)
|
|
{
|
|
if (fabs(start_point[t] - end_point[t]) > 0.001f)
|
|
res = false;
|
|
}
|
|
// checking that transforming a point with extrinsiscs from i to j and the from j to i
|
|
// gets back to the initial point
|
|
REQUIRE(res);
|
|
|
|
// checking that a point and orientation, represented by a 4x4 matrix
|
|
// is equal to the result of transforming it by the extrinsics
|
|
// from profile A to B, and then from profile B to A
|
|
position_and_rotation point_and_orientation;
|
|
// rotation part with 30 degrees rotation on each axis
|
|
point_and_orientation.pos_and_rot[0][0] = 0.75;
|
|
point_and_orientation.pos_and_rot[0][1] = -0.4330127;
|
|
point_and_orientation.pos_and_rot[0][2] = 0.5;
|
|
point_and_orientation.pos_and_rot[1][0] = 0.649519;
|
|
point_and_orientation.pos_and_rot[1][1] = 0.625;
|
|
point_and_orientation.pos_and_rot[1][2] = -0.4330127;
|
|
point_and_orientation.pos_and_rot[2][0] = -0.125;
|
|
point_and_orientation.pos_and_rot[2][1] = 0.649519;
|
|
point_and_orientation.pos_and_rot[2][2] = 0.75;
|
|
|
|
point_and_orientation.pos_and_rot[3][0] = 0.0;
|
|
point_and_orientation.pos_and_rot[3][1] = 0.0;
|
|
point_and_orientation.pos_and_rot[3][2] = 0.0;
|
|
|
|
// translation part
|
|
point_and_orientation.pos_and_rot[0][3] = 1.0;
|
|
point_and_orientation.pos_and_rot[1][3] = 2.0;
|
|
point_and_orientation.pos_and_rot[2][3] = 3.0;
|
|
|
|
point_and_orientation.pos_and_rot[3][3] = 1.0;
|
|
|
|
// applying extrinsics from i to j on point with orientation
|
|
position_and_rotation retransformed_temp = pr_j_to_i * point_and_orientation;
|
|
// applying extrinsics from j to i
|
|
position_and_rotation retransformed = pr_i_to_j * retransformed_temp;
|
|
// checking that the point and orientation are the same as before the transformations
|
|
REQUIRE(retransformed.equals(point_and_orientation));
|
|
}
|
|
}
|
|
}
|
|
}
|