TEST_F(TrajectoryLibraryTest, RemainderTrajectory) {
StereoOctomap octomap(bot_frames_);
// Load a complicated trajectory
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/many", true);
double altitude = 30;
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[2] = altitude;
const Trajectory *traj = lib.GetTrajectoryByNumber(1);
double dist;
double point[3] = { 6.65, -7.23, 9.10 };
AddPointToOctree(&octomap, point, altitude);
dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
EXPECT_NEAR(dist, 11.748141101535500, TOLERANCE2); // from matlab
dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0.95, 0);
EXPECT_NEAR(dist, 11.8831, TOLERANCE2);
dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 1.5, 0); // t after trajectory
EXPECT_NEAR(dist, 12.3832, TOLERANCE2); // from matlab
}
TEST_F(TrajectoryLibraryTest, ManyPointsAgainstMatlab) {
StereoOctomap octomap(bot_frames_);
double altitude = 30;
// load points
AddManyPointsToOctree(&octomap, x_points_, y_points_, z_points_, number_of_reference_points_, altitude);
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/many", true);
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[2] = altitude;
double dist;
const Trajectory *best_traj;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 50.0);
ASSERT_TRUE(best_traj != nullptr);
EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 1);
EXPECT_NEAR(dist, 4.2911, 0.001);
}
TEST_F(TrajectoryLibraryTest, RemainderTrajectorySimpleAltitude) {
StereoOctomap octomap(bot_frames_);
// get a trajectory
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/simple", true);
double altitude = 5;
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[2] = altitude;
const Trajectory *traj = lib.GetTrajectoryByNumber(0);
double dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
EXPECT_EQ_ARM(dist, -1);
// add an obstacle
double point[3] = { 0, 0, 6};
AddPointToOctree(&octomap, point, 0);
dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
EXPECT_EQ_ARM(dist, 6 - altitude);
}
TEST_F(TrajectoryLibraryTest, ManyTrajectoriesWithTransform) {
StereoOctomap octomap(bot_frames_);
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/full", true);
double altitude = 30;
double point[3] = {18, 12, 0};
AddPointToOctree(&octomap, point, altitude);
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[0] = 17;
trans.trans_vec[1] = 11;
trans.trans_vec[2] = altitude;
// send th
double dist;
const Trajectory *best_traj;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0);
DrawOriginLcmGl(lcm_);
bot_lcmgl_color3f(lcmgl_, 0, 0, 1);
best_traj->Draw(lcmgl_, &trans);
EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 3);
EXPECT_NEAR(dist, 1.025243, TOLERANCE);
// now add a yaw
trans.rot_quat[0] = 0.642787609686539;
trans.rot_quat[1] = 0;
trans.rot_quat[2] = 0;
trans.rot_quat[3] = 0.766044443118978;
bot_lcmgl_color3f(lcmgl_, 1, 0, 0);
best_traj->Draw(lcmgl_, &trans);
bot_lcmgl_switch_buffer(lcmgl_);
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0);
//lib.Draw(lcm_, &trans);
EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 2);
EXPECT_NEAR(dist, 1.174604, TOLERANCE);
// now have a transform with roll, pitch, and yaw
trans.rot_quat[0] = 0.863589399067779;
trans.rot_quat[1] = -0.004581450790098;
trans.rot_quat[2] = 0.298930259006064;
trans.rot_quat[3] = 0.405996379758463;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0);
EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 4);
EXPECT_NEAR(dist, 0.327772, TOLERANCE);
}
void
bot_trans_set_from_velocities(BotTrans *dest, const double angular_rate[3],
const double velocity[3], double dt)
{
//see Frazolli notes, Aircraft Stability and Control, lecture 2, page 15
if (dt == 0) {
bot_trans_set_identity(dest);
return;
}
double identity_quat[4] = { 1, 0, 0, 0 };
double norm_angular_rate = bot_vector_magnitude_3d(angular_rate);
if (norm_angular_rate == 0) {
double delta[3];
memcpy(delta, velocity, 3 * sizeof(double));
bot_vector_scale_3d(delta, dt);
bot_trans_set_from_quat_trans(dest, identity_quat, delta);
return;
}
//"exponential of a twist: R=exp(skew(omega*t));
//rescale vel, omega, t so ||omega||=1
//trans = (I-R)*(omega \cross v) + (omega \dot v) *omega* t
// term2 term3
// R*(omega \cross v)
// term1
//rescale
double t = dt * norm_angular_rate;
double omega[3], vel[3];
memcpy(omega, angular_rate, 3 * sizeof(double));
memcpy(vel, velocity, 3 * sizeof(double));
bot_vector_scale_3d(omega, 1.0/norm_angular_rate);
bot_vector_scale_3d(vel, 1.0/norm_angular_rate);
//compute R (quat in our case)
bot_angle_axis_to_quat(t, omega, dest->rot_quat);
//cross and dot products
double omega_cross_vel[3];
bot_vector_cross_3d(omega, vel, omega_cross_vel);
double omega_dot_vel = bot_vector_dot_3d(omega, vel);
double term1[3];
double term2[3];
double term3[3];
//(I-R)*(omega \cross v) = term2
memcpy(term1, omega_cross_vel, 3 * sizeof(double));
bot_quat_rotate(dest->rot_quat, term1);
bot_vector_subtract_3d(omega_cross_vel, term1, term2);
//(omega \dot v) *omega* t
memcpy(term3, omega, 3 * sizeof(double));
bot_vector_scale_3d(term3, omega_dot_vel * t);
bot_vector_add_3d(term2, term3, dest->trans_vec);
}
TEST_F(TrajectoryLibraryTest, TimingTest) {
StereoOctomap octomap(bot_frames_);
double altitude = 30;
// create a random point cloud
int num_points = 10000;
int num_lookups = 1000;
std::uniform_real_distribution<double> uniform_dist(-1000, 1000);
std::random_device rd;
std::default_random_engine rand_engine(rd());
float x[num_points], y[num_points], z[num_points];
for (int i = 0; i < num_points; i++) {
// generate a random point
x[i] = uniform_dist(rand_engine);
y[i] = uniform_dist(rand_engine);
z[i] = uniform_dist(rand_engine);
//std::cout << "Point: (" << this_point[0] << ", " << this_point[1] << ", " << this_point[2] << ")" << std::endl;
}
AddManyPointsToOctree(&octomap, x, y, z, num_points, altitude);
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/many", true);
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[2] = altitude;
tic();
for (int i = 0; i < num_lookups; i++) {
double dist;
const Trajectory *best_traj;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 50.0);
}
double num_sec = toc();
std::cout << num_lookups << " lookups with " << lib.GetNumberTrajectories() << " trajectories on a cloud (" << num_points << ") took: " << num_sec << " sec (" << num_sec / (double)num_lookups*1000.0 << " ms / lookup)" << std::endl;
}
/**
* Tests point transformation with a simple two-point trajectory
*/
TEST_F(TrajectoryLibraryTest, GetTransformedPoint) {
Trajectory traj("trajtest/simple/two-point-00000", true);
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[0] = 1;
double point[3] = {1, 0, 0};
double output[3];
traj.GetXyzYawTransformedPoint(0, trans, output);
//std::cout << "point = (" << point[0] << ", " << point[1] << ", " << point[2] << ")" << std::endl;
//std::cout << "output = (" << output[0] << ", " << output[1] << ", " << output[2] << ")" << std::endl;
for (int i = 0; i < 3; i++) {
EXPECT_NEAR(point[i], output[i], TOLERANCE);
}
trans.trans_vec[0] = 0;
traj.GetXyzYawTransformedPoint(1, trans, output);
for (int i = 0; i < 3; i++) {
EXPECT_NEAR(point[i], output[i], TOLERANCE);
}
// transform with rotation
trans.rot_quat[0] = 0.707106781186547;
trans.rot_quat[1] = 0;
trans.rot_quat[2] = 0;
trans.rot_quat[3] = 0.707106781186547;
traj.GetXyzYawTransformedPoint(1, trans, output);
point[0] = 0;
point[1] = 1;
point[2] = 0;
//std::cout << "point = (" << point[0] << ", " << point[1] << ", " << point[2] << ")" << std::endl;
//std::cout << "output = (" << output[0] << ", " << output[1] << ", " << output[2] << ")" << std::endl;
for (int i = 0; i < 3; i++) {
EXPECT_NEAR(point[i], output[i], TOLERANCE);
}
}
TEST_F(TrajectoryLibraryTest, CheckBounds) {
Trajectory traj("trajtest/simple/two-point-00000", true);
BotTrans trans;
bot_trans_set_identity(&trans);
double output[3];
traj.GetXyzYawTransformedPoint(0, trans, output);
EXPECT_EQ_ARM(output[0], 0);
traj.GetXyzYawTransformedPoint(0.01, trans, output);
EXPECT_EQ_ARM(output[0], 1);
traj.GetXyzYawTransformedPoint(10, trans, output);
EXPECT_EQ_ARM(output[0], 1);
}
int
bot_ctrans_path_to_trans_latest(const BotCTransPath * path, BotTrans *result)
{
bot_trans_set_identity(result);
BotTrans temp_trans;
for(int lind=0; lind<path->nlinks; lind++) {
BotCTransLink *link = path->links[lind];
int have_trans = _link_get_trans_latest(link, &temp_trans);
if(!have_trans) {
return 0;
}
if(path->invert[lind]) {
bot_trans_invert(&temp_trans);
}
bot_trans_apply_trans(result, &temp_trans);
}
return 1;
}
TEST_F(TrajectoryLibraryTest, Altitude) {
StereoOctomap octomap(bot_frames_);
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/simple", true);
double altitude = 0.5;
double point[3] = { 1.05, 0, 0 };
AddPointToOctree(&octomap, point, altitude);
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[2] = altitude;
double dist;
const Trajectory *best_traj;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0);
EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 1);
}
TEST_F(TrajectoryLibraryTest, FindFarthestWithTI) {
StereoOctomap octomap(bot_frames_);
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/full", true);
double altitude = 30;
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[2] = altitude;
double dist;
const Trajectory *best_traj;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 50.0);
ASSERT_TRUE(best_traj != nullptr);
// with no obstacles, we should always get the first trajectory
EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 0);
// put an obstacle along the path of the TI trajectory
double point[3] = { 1.10, -0.1, 0 };
AddPointToOctree(&octomap, point, altitude);
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 50.0);
EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 4); // from matlab
EXPECT_NEAR(dist, 0.2032, TOLERANCE2); // from matlab
// create a new octre
StereoOctomap octomap2(bot_frames_);
AddManyPointsToOctree(&octomap2, x_points_, y_points_, z_points_, number_of_reference_points_, altitude);
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap2, trans, 50.0);
EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 3);
EXPECT_NEAR(dist, 5.0060, TOLERANCE);
}
TEST_F(TrajectoryLibraryTest, TwoTrajectoriesOnePointWithTransform) {
StereoOctomap octomap(bot_frames_);
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/simple", true);
double altitude = 30;
double point[3] = { 1.05, 0, 0 };
AddPointToOctree(&octomap, point, altitude);
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[2] = altitude;
double dist;
const Trajectory *best_traj;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0);
EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 1);
trans.rot_quat[0] = 0.8349;
trans.rot_quat[1] = 0.3300;
trans.rot_quat[2] = 0.4236;
trans.rot_quat[3] = -0.1206;
// rotation shouldn't matter
// TODO: this will fail when I introduce aircraft rotation into the check
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0);
EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 1);
// with a translation, we expect a different result
trans.trans_vec[0] = 1;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0);
EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 0);
}
TEST_F(TrajectoryLibraryTest, RemainderTrajectoryTi) {
StereoOctomap octomap(bot_frames_);
double altitude = 30;
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[2] = altitude;
Trajectory traj("trajtest/ti/TI-test-TI-straight-pd-no-yaw-00000", true);
double dist = traj.ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
EXPECT_EQ_ARM(dist, -1);
double point[3] = { 1.5, -0.5, 3 };
AddPointToOctree(&octomap, point, altitude);
dist = traj.ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
EXPECT_NEAR(dist, 3.041506, TOLERANCE2); // from matlab
dist = traj.ClosestObstacleInRemainderOfTrajectory(octomap, trans, 1.21, 0);
EXPECT_NEAR(dist, 13.6886, TOLERANCE2);
}
TEST_F(TrajectoryLibraryTest, RemainderTrajectorySimple) {
StereoOctomap octomap(bot_frames_);
// get a trajectory
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/simple", true);
double altitude = 100;
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[2] = altitude;
const Trajectory *traj = lib.GetTrajectoryByNumber(0);
double dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
// add an obstacle
double point[3] = { 0, 0, 0};
AddPointToOctree(&octomap, point, altitude);
// now we expect zero distance since the obstacle and the trajectory start at the origin
dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
EXPECT_NEAR(dist, 0, TOLERANCE);
// at t = 0.01, we expect a distance of 1 since the point is already behind us
dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0.01, 0);
EXPECT_EQ_ARM(dist, 1);
// add a transform
trans.trans_vec[2] += 42;
dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
EXPECT_EQ_ARM(dist, 42);
dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0.01, 0);
EXPECT_NEAR(dist, sqrt( 42*42 + 1*1 ), TOLERANCE);
}
/**
* Test FindFarthestTrajectory on:
* - no obstacles
* - one obstacle
* - two obstacles
*/
TEST_F(TrajectoryLibraryTest, FindFarthestTrajectory) {
StereoOctomap octomap(bot_frames_);
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/simple", true);
EXPECT_EQ_ARM(lib.GetNumberTrajectories(), 2);
BotTrans trans;
bot_trans_set_identity(&trans);
double altitude = 30;
trans.trans_vec[2] = altitude; // flying high up
// check that things work with no obstacles (should return first trajectory)
double dist;
const Trajectory *best_traj;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0);
ASSERT_TRUE(best_traj != nullptr);
EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 0);
// check for preferred trajectory
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0, nullptr, 1);
ASSERT_TRUE(best_traj != nullptr);
EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 1);
// preferred trajectory error
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0, nullptr, 2);
ASSERT_TRUE(best_traj != nullptr);
EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 0);
// add an obstacle close to the first trajectory
double point[3] = { 1.05, 0, 0 };
AddPointToOctree(&octomap, point, altitude);
// now we expect to get the second trajectory
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0);
ASSERT_TRUE(best_traj != nullptr);
EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 1);
EXPECT_NEAR(dist, 2.0 - 1.05, TOLERANCE);
// add another point close to the second trajectory
point[0] = 2.01;
point[1] = 0.01;
point[2] = -0.015;
AddPointToOctree(&octomap, point, altitude);
// now we expect to get the first trajectory
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0);
ASSERT_TRUE(best_traj != nullptr);
EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 0);
EXPECT_NEAR(dist, 0.05, TOLERANCE);
}