bool Mapper::getBoundedMap(ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res)
{
if (!mapPointCloud)
return false;
const float max_x = req.topRightCorner.x;
const float max_y = req.topRightCorner.y;
const float max_z = req.topRightCorner.z;
const float min_x = req.bottomLeftCorner.x;
const float min_y = req.bottomLeftCorner.y;
const float min_z = req.bottomLeftCorner.z;
cerr << "min [" << min_x << ", " << min_y << ", " << min_z << "] " << endl;
cerr << "max [" << max_x << ", " << max_y << ", " << max_z << "] " << endl;
tf::StampedTransform stampedTr;
Eigen::Affine3d eigenTr;
tf::poseMsgToEigen(req.mapCenter, eigenTr);
Eigen::MatrixXf T = eigenTr.matrix().inverse().cast<float>();
//const Eigen::MatrixXf T = eigenTr.matrix().cast<float>();
cerr << "T:" << endl << T << endl;
T = transformation->correctParameters(T);
// FIXME: do we need a mutex here?
const DP centeredPointCloud = transformation->compute(*mapPointCloud, T);
DP cutPointCloud = centeredPointCloud.createSimilarEmpty();
cerr << centeredPointCloud.features.topLeftCorner(3, 10) << endl;
cerr << T << endl;
int newPtCount = 0;
for(int i=0; i < centeredPointCloud.features.cols(); i++)
{
const float x = centeredPointCloud.features(0,i);
const float y = centeredPointCloud.features(1,i);
const float z = centeredPointCloud.features(2,i);
if(x < max_x && x > min_x &&
y < max_y && y > min_y &&
z < max_z && z > min_z )
{
cutPointCloud.setColFrom(newPtCount, centeredPointCloud, i);
newPtCount++;
}
}
cerr << "Extract " << newPtCount << " points from the map" << endl;
cutPointCloud.conservativeResize(newPtCount);
cutPointCloud = transformation->compute(cutPointCloud, T.inverse());
// Send the resulting point cloud in ROS format
res.boundedMap = PointMatcher_ros::pointMatcherCloudToRosMsg<float>(cutPointCloud, mapFrame, ros::Time::now());
return true;
}
TEST(IOTest, loadPLY)
{
typedef PointMatcherIO<float> IO;
std::istringstream is;
is.str(
""
);
EXPECT_THROW(IO::loadPLY(is), runtime_error);
is.clear();
is.str(
"ply\n"
"format binary_big_endian 1.0\n"
);
EXPECT_THROW(IO::loadPLY(is), runtime_error);
is.clear();
is.str(
"ply\n"
"format ascii 2.0\n"
);
EXPECT_THROW(IO::loadPLY(is), runtime_error);
is.clear();
is.str(
"ply\n"
"format ascii 1.0\n"
);
EXPECT_THROW(IO::loadPLY(is), runtime_error);
is.clear();
is.str(
"ply\n"
"format ascii 1.0\n"
"element vertex 5\n"
"\n" //empty line
"property float z\n" // wrong order
"property float y\n"
"property float x\n"
"property float grrrr\n" //unknown property
"property float nz\n" // wrong order
"property float ny\n"
"property float nx\n"
"end_header\n"
"3 2 1 99 33 22 11\n"
"3 2 1 99 33 22 11\n"
"\n" //empty line
"3 2 1 99 33 22 11 3 2 1 99 33 22 11\n" // no line break
"3 2 1 99 33 22 11\n"
);
DP pointCloud = IO::loadPLY(is);
// Confirm sizes and dimensions
EXPECT_TRUE(pointCloud.features.cols() == 5);
EXPECT_TRUE(pointCloud.features.rows() == 4);
EXPECT_TRUE(pointCloud.descriptors.cols() == 5);
EXPECT_TRUE(pointCloud.descriptors.rows() == 3);
// Random value check
EXPECT_TRUE(pointCloud.features(0, 0) == 1);
EXPECT_TRUE(pointCloud.features(2, 2) == 3);
EXPECT_TRUE(pointCloud.descriptors(1, 1) == 22);
EXPECT_TRUE(pointCloud.descriptors(2, 4) == 33);
}
