int main() {
int r=0;
printf("--------------------------------------------------------------------------------\n");
printf(" VC2HQEncode Test Suite\n");
printf("--------------------------------------------------------------------------------\n");
detect_cpu_features();
r = test_transforms(HAS_SSE4_2, HAS_AVX, HAS_AVX2);
if (r) return r;
return r;
}
int main()
{
// See: http://www.ros.org/wiki/tf
tf::Transform world_to_car;
world_to_car.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
world_to_car.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
tf::Transform car_to_gps;
car_to_gps.setOrigin(tf::Vector3(-1.0, 0.0, 0.0));
car_to_gps.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
tf::Transform car_to_xsens;
car_to_xsens.setOrigin(tf::Vector3(0.5, 0.2, 0.0));
car_to_xsens.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
tf::Transform car_to_camera;
car_to_camera.setOrigin(tf::Vector3(0.5, -0.2, 0.0));
car_to_camera.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
tf::Time::init();
tf::Transformer transformer(false);
transformer.setTransform(tf::StampedTransform(world_to_car, tf::Time(0), "/world", "/car"));
transformer.setTransform(tf::StampedTransform(car_to_gps, tf::Time(0), "/car", "/gps"));
transformer.setTransform(tf::StampedTransform(car_to_xsens, tf::Time(0), "/car", "/xsens"));
transformer.setTransform(tf::StampedTransform(car_to_camera, tf::Time(0), "/car", "/camera"));
// ----------------------------- //
//printf("Initial Poses:\n");
tf::StampedTransform camera_to_world;
transformer.lookupTransform("/world", "/camera", tf::Time(0), camera_to_world);
//printf("Camera pose with respect to world : x: % 6.2f, y: % 6.2f, z: % 6.2f\n", camera_to_world.getOrigin().x(), camera_to_world.getOrigin().y(), camera_to_world.getOrigin().z());
for (double z = 0.0; z < 10.0; z += 1.0)
{
world_to_car.setOrigin(tf::Vector3(z, 0.0, 0.0));
world_to_car.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
transformer.setTransform(tf::StampedTransform(world_to_car, tf::Time(0), "/world", "/car"));
transformer.lookupTransform("/world", "/camera", tf::Time(0), camera_to_world);
//printf("Camera pose with respect to world : x: % 6.2f, y: % 6.2f, z: % 6.2f\n", camera_to_world.getOrigin().x(), camera_to_world.getOrigin().y(), camera_to_world.getOrigin().z());
}
transformer.lookupTransform("/car", "/gps", tf::Time(0), camera_to_world);
//printf("Gps pose with respect to car : x: % 6.2f, y: % 6.2f, z: % 6.2f\n", camera_to_world.getOrigin().x(), camera_to_world.getOrigin().y(), camera_to_world.getOrigin().z());
tf::Transform world_to_gps;
world_to_gps.setOrigin(tf::Vector3(-1.0, 0.0, 0.0));
world_to_gps.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
transformer.setTransform(tf::StampedTransform(car_to_gps, tf::Time(0), "/world", "/gps"));
transformer.lookupTransform("/car", "/gps", tf::Time(0), camera_to_world);
//printf("Gps pose with respect to car : x: % 6.2f, y: % 6.2f, z: % 6.2f\n", camera_to_world.getOrigin().x(), camera_to_world.getOrigin().y(), camera_to_world.getOrigin().z());
test_transforms();
return 0;
}
