void AddManyPointsToOctree(StereoOctomap *octomap, float x_in[], float y_in[], float z_in[], int number_of_points, double altitude_offset) {
lcmt::stereo msg;
msg.timestamp = GetTimestampNow();
vector<float> x, y, z;
for (int i = 0; i < number_of_points; i++) {
double this_point[3];
this_point[0] = x_in[i];
this_point[1] = y_in[i];
this_point[2] = z_in[i] + altitude_offset;
double point_transformed[3];
GlobalToCameraFrame(this_point, point_transformed);
//std::cout << "Point: (" << point_transformed[0] << ", " << point_transformed[1] << ", " << point_transformed[2] << ")" << std::endl;
x.push_back(point_transformed[0]);
y.push_back(point_transformed[1]);
z.push_back(point_transformed[2]);
}
msg.x = x;
msg.y = y;
msg.z = z;
msg.number_of_points = number_of_points;
msg.video_number = 0;
msg.frame_number = 0;
octomap->ProcessStereoMessage(&msg);
}
void SendStereoManyPointsTriple(vector<float> x_in, vector<float> y_in, vector<float> z_in) {
lcmt::stereo msg;
msg.timestamp = GetTimestampNow();
vector<float> x, y, z;
vector<unsigned char> grey;
for (int i = 0; i < (int)x_in.size(); i++) {
double this_point[3];
this_point[0] = x_in[i];
this_point[1] = y_in[i];
this_point[2] = z_in[i];
double point_transformed[3];
GlobalToCameraFrame(this_point, point_transformed);
//std::cout << "Point: (" << point_transformed[0] << ", " << point_transformed[1] << ", " << point_transformed[2] << ")" << std::endl;
x.push_back(point_transformed[0]);
y.push_back(point_transformed[1]);
z.push_back(point_transformed[2]);
grey.push_back(0);
x.push_back(point_transformed[0]);
y.push_back(point_transformed[1]);
z.push_back(point_transformed[2]);
grey.push_back(0);
x.push_back(point_transformed[0]);
y.push_back(point_transformed[1]);
z.push_back(point_transformed[2]);
grey.push_back(0);
}
msg.x = x;
msg.y = y;
msg.z = z;
msg.grey = grey;
msg.number_of_points = x.size();
msg.video_number = 0;
msg.frame_number = 0;
lcm_->publish("stereo", &msg);
}
TEST_F(StereoOctomapTest, SimpleNearestNeighbor) {
StereoOctomap *stereo_octomap = new StereoOctomap(bot_frames_);
// first test when no points are there
double origin[3];
origin[0] = 0;
origin[1] = 0;
origin[2] = 0;
EXPECT_TRUE(stereo_octomap->NearestNeighbor(origin) == -1) << "No points in octomap failed." << std::endl;
// add a point
double point[3], trans_point[3];
point[0] = 1;
point[1] = 0;
point[2] = 0;
GlobalToCameraFrame(point, trans_point);
lcmt::stereo msg;
msg.timestamp = GetTimestampNow();
vector<float> x, y, z;
x.push_back(trans_point[0]);
y.push_back(trans_point[1]);
z.push_back(trans_point[2]);
msg.x = x;
msg.y = y;
msg.z = z;
msg.number_of_points = 1;
msg.frame_number = 0;
msg.video_number = 0;
stereo_octomap->ProcessStereoMessage(&msg);
EXPECT_EQ_ARM(stereo_octomap->NearestNeighbor(origin), 1);
EXPECT_EQ_ARM(stereo_octomap->NearestNeighbor(point), 0);
double point2[3] = {0, 0, 1};
EXPECT_NEAR(stereo_octomap->NearestNeighbor(point2), sqrt(2), TOLERANCE);
lcmt::stereo msg2;
msg2.timestamp = GetTimestampNow();
vector<float> x_2, y_2, z_2;
double trans_point2[3];
point[0] = 0;
point[1] = 2;
point[2] = 0;
GlobalToCameraFrame(point, trans_point2);
x_2.push_back(trans_point2[0]);
y_2.push_back(trans_point2[1]);
z_2.push_back(trans_point2[2]);
msg2.x = x_2;
msg2.y = y_2;
msg2.z = z_2;
msg2.number_of_points = 1;
msg2.frame_number = 0;
msg2.video_number = 0;
// add the points to the octomap
stereo_octomap->ProcessStereoMessage(&msg2);
// check
EXPECT_NEAR(stereo_octomap->NearestNeighbor(origin), 1, TOLERANCE);
double query[3] = { 1.5, 0, 0 };
EXPECT_NEAR(stereo_octomap->NearestNeighbor(query), 0.5, TOLERANCE);
query[0] = 1;
query[1] = 0;
query[2] = 1;
// check z
EXPECT_NEAR(stereo_octomap->NearestNeighbor(query), 1, TOLERANCE);
query[0] = 0;
query[1] = 2.01;
query[2] = 0;
EXPECT_NEAR(stereo_octomap->NearestNeighbor(query), 0.01, TOLERANCE);
query[0] = 0;
query[1] = 2.1;
query[2] = 0.5;
EXPECT_NEAR(stereo_octomap->NearestNeighbor(query), sqrt(0.1*0.1 + 0.5*0.5), TOLERANCE);
query[0] = -1;
query[1] = 2.5;
query[2] = 1.5;
EXPECT_NEAR(stereo_octomap->NearestNeighbor(query), sqrt(1*1 + .5*.5 + 1.5*1.5), TOLERANCE);
delete stereo_octomap;
}
TEST_F(StereoOctomapTest, CheckAgainstLinearSearch) {
int num_points = 10000;
vector<float> x;
vector<float> y;
vector<float> z;
StereoOctomap *stereo_octomap = new StereoOctomap(bot_frames_);
// create a random point cloud
std::uniform_real_distribution<double> uniform_dist(-1000, 1000);
std::random_device rd;
std::default_random_engine rand_engine(rd());
for (int i = 0; i < num_points; i++) {
double this_point[3];
// generate a random point
this_point[0] = uniform_dist(rand_engine);
this_point[1] = uniform_dist(rand_engine);
this_point[2] = uniform_dist(rand_engine);
//std::cout << "Point: (" << this_point[0] << ", " << this_point[1] << ", " << this_point[2] << ")" << std::endl;
double translated_point[3];
GlobalToCameraFrame(this_point, translated_point);
x.push_back(translated_point[0]);
y.push_back(translated_point[1]);
z.push_back(translated_point[2]);
}
lcmt::stereo msg;
msg.timestamp = GetTimestampNow();
msg.x = x;
msg.y = y;
msg.z = z;
msg.number_of_points = num_points;
msg.frame_number = 0;
msg.video_number = 0;
stereo_octomap->ProcessStereoMessage(&msg);
// now perform a bunch of searches
double time_linear = 0, time_octomap = 0;
int num_searches = 1000;
for (int i = 0; i < num_searches; i++) {
double search_point[3];
// generate a random point
search_point[0] = uniform_dist(rand_engine);
search_point[1] = uniform_dist(rand_engine);
search_point[2] = uniform_dist(rand_engine);
//std::cout << "search_point: (" << search_point[0] << ", " << search_point[1] << ", " << search_point[2] << ")" << std::endl;
tic();
double linear_search_dist = NearestNeighborLinear(x, y, z, search_point);
time_linear += toc();
tic();
double octomap_dist = stereo_octomap->NearestNeighbor(search_point);
time_octomap += toc();
EXPECT_NEAR(octomap_dist, linear_search_dist, TOLERANCE);
}
std::cout << "\tFor " << num_searches << " searches over " << num_points << " points: " << std:: endl << "\t\tlinear time = " << time_linear << ", octree time = " << time_octomap << ", for a speedup of: " << time_linear / time_octomap << "x" << std::endl;
delete stereo_octomap;
}
