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); }