i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 01:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added LASWriter tests

Browse Source
This commit is contained in:
matlabbe
2026-05-17 14:58:35 -07:00
parent 057d385404
commit 7ccc41de12
4 changed files with 229 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/LASWriter.h
+45 -4
View File
@@ -28,16 +28,57 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <string>
#include <vector>
namespace rtabmap {
int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud, const std::vector<int> & cameraIds = std::vector<int>());
int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), const std::vector<float> & intensities = std::vector<float>());
int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud, const std::vector<int> & cameraIds = std::vector<int>());
/**
* @brief Writes a PCL point cloud to a LAS/LAZ file (requires RTAB-Map built with libLAS).
*
* Output uses 1 mm XYZ scale (`0.001`). The file extension (`.las` or `.laz`) selects
* uncompressed or compressed output when libLAS LAZ support is available.
*
* @param filePath Output path (`.las` or `.laz`).
* @param cloud Input point cloud.
* @param cameraIds Optional per-point camera/signature ids (stored as point source ID);
* must be empty or the same length as @p cloud.
* @return `0` on success, `1` on error (e.g. LAZ not supported).
*/
int RTABMAP_CORE_EXPORT saveLASFile(
const std::string & filePath,
const pcl::PointCloud<pcl::PointXYZ> & cloud,
const std::vector<int> & cameraIds = std::vector<int>());
}
/**
* @brief Writes a colored point cloud to LAS/LAZ, with optional intensity and camera ids.
* @param filePath Output path (`.las` or `.laz`).
* @param cloud RGB point cloud (8-bit channels mapped to 16-bit LAS color).
* @param cameraIds Optional per-point ids (same length as @p cloud or empty).
* @param intensities Optional per-point intensities (same length as @p cloud or empty).
* @return `0` on success, `1` on error.
*/
int RTABMAP_CORE_EXPORT saveLASFile(
const std::string & filePath,
const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
const std::vector<int> & cameraIds = std::vector<int>(),
const std::vector<float> & intensities = std::vector<float>());
/**
* @brief Writes a point cloud with intensity channel to LAS/LAZ.
* @param filePath Output path (`.las` or `.laz`).
* @param cloud XYZI point cloud (coordinates and intensity written).
* @param cameraIds Optional per-point ids (same length as @p cloud or empty).
* @return `0` on success, `1` on error.
*/
int RTABMAP_CORE_EXPORT saveLASFile(
const std::string & filePath,
const pcl::PointCloud<pcl::PointXYZI> & cloud,
const std::vector<int> & cameraIds = std::vector<int>());
} // namespace rtabmap
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_ */
corelib/src/LASWriter.cpp
+2 -1
View File
@@ -148,11 +148,12 @@ int saveLASFile(const std::string & filePath,
{
liblas::Point point(&header);
point.SetCoordinates(cloud.at(i).x, cloud.at(i).y, cloud.at(i).z);
point.SetIntensity(cloud.at(i).intensity);
if(!cameraIds.empty())
{
point.SetPointSourceID(cameraIds.at(i));
}
writer.WritePoint(point);
}
}
corelib/test/CMakeLists.txt
+8
View File
@@ -126,6 +126,14 @@ add_executable(test_landmark test_landmark.cpp)
target_link_libraries(test_landmark gtest_main rtabmap_core)
add_test(NAME test_landmark COMMAND test_landmark)
#LASWriter.h (optional libLAS)
IF(libLAS_FOUND)
add_executable(test_laswriter test_laswriter.cpp)
target_link_libraries(test_laswriter gtest_main rtabmap_core ${libLAS_LIBRARIES})
target_include_directories(test_laswriter PRIVATE ${libLAS_INCLUDE_DIRS})
add_test(NAME test_laswriter COMMAND test_laswriter)
ENDIF(libLAS_FOUND)
#Graph.h
add_executable(test_graph test_graph.cpp)
target_link_libraries(test_graph gtest_main rtabmap_core)
corelib/test/test_laswriter.cpp
+174
View File
@@ -0,0 +1,174 @@
#include <gtest/gtest.h>
#include <rtabmap/core/Version.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
#include <fstream>
#include <unistd.h>
#ifdef RTABMAP_LIBLAS
#include <rtabmap/core/LASWriter.h>
#include <liblas/liblas.hpp>
#endif
using namespace rtabmap;
namespace {
static int g_fileCounter = 0;
static std::string tempLasPath(const std::string & extension)
{
return uFormat("/tmp/rtabmap_laswriter_test_%d_%d.%s", getpid(), ++g_fileCounter, extension.c_str());
}
#ifdef RTABMAP_LIBLAS
static void expectLasSignature(const std::string & path)
{
std::ifstream file(path.c_str(), std::ios::binary);
ASSERT_TRUE(file.good());
char signature[4] = {0};
file.read(signature, 4);
EXPECT_EQ(signature[0], 'L');
EXPECT_EQ(signature[1], 'A');
EXPECT_EQ(signature[2], 'S');
EXPECT_EQ(signature[3], 'F');
}
static void readLasPointCount(const std::string & path, size_t & count)
{
std::ifstream ifs(path.c_str(), std::ios::in | std::ios::binary);
ASSERT_TRUE(ifs.good());
liblas::ReaderFactory factory;
liblas::Reader reader = factory.CreateWithStream(ifs);
count = reader.GetHeader().GetPointRecordsCount();
}
#endif // RTABMAP_LIBLAS
} // namespace
#ifndef RTABMAP_LIBLAS
TEST(LASWriterTest, SkippedWithoutLibLAS)
{
GTEST_SKIP() << "RTAB-Map not built with libLAS support";
}
#else
TEST(LASWriterTest, SaveXYZCloud)
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.push_back(pcl::PointXYZ(1.f, 2.f, 3.f));
cloud.push_back(pcl::PointXYZ(4.f, 5.f, 6.f));
const std::string path = tempLasPath("las");
EXPECT_EQ(saveLASFile(path, cloud), 0);
expectLasSignature(path);
size_t count = 0;
ASSERT_NO_FATAL_FAILURE(readLasPointCount(path, count));
EXPECT_EQ(count, 2u);
std::ifstream ifs(path.c_str(), std::ios::in | std::ios::binary);
liblas::ReaderFactory factory;
liblas::Reader reader = factory.CreateWithStream(ifs);
ASSERT_TRUE(reader.ReadNextPoint());
EXPECT_NEAR(reader.GetPoint().GetX(), 1.0, 1e-3);
EXPECT_NEAR(reader.GetPoint().GetY(), 2.0, 1e-3);
EXPECT_NEAR(reader.GetPoint().GetZ(), 3.0, 1e-3);
UFile::erase(path);
}
TEST(LASWriterTest, SaveXYZCloudLaz)
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.push_back(pcl::PointXYZ(1.f, 2.f, 3.f));
cloud.push_back(pcl::PointXYZ(4.f, 5.f, 6.f));
const std::string path = tempLasPath("laz");
const int rc = saveLASFile(path, cloud);
if(rc == 1)
{
// libLAS built without LASzip (common on distro packages).
EXPECT_EQ(rc, 1);
UFile::erase(path);
return;
}
ASSERT_EQ(rc, 0);
size_t count = 0;
ASSERT_NO_FATAL_FAILURE(readLasPointCount(path, count));
EXPECT_EQ(count, 2u);
std::ifstream ifs(path.c_str(), std::ios::in | std::ios::binary);
liblas::ReaderFactory factory;
liblas::Reader reader = factory.CreateWithStream(ifs);
ASSERT_TRUE(reader.ReadNextPoint());
EXPECT_NEAR(reader.GetPoint().GetX(), 1.0, 1e-3);
EXPECT_NEAR(reader.GetPoint().GetY(), 2.0, 1e-3);
EXPECT_NEAR(reader.GetPoint().GetZ(), 3.0, 1e-3);
UFile::erase(path);
}
TEST(LASWriterTest, SaveRGBCloudWithCameraIdsAndIntensity)
{
pcl::PointCloud<pcl::PointXYZRGB> cloud;
pcl::PointXYZRGB pt;
pt.x = 0.f;
pt.y = 0.f;
pt.z = 0.f;
pt.r = 255;
pt.g = 128;
pt.b = 64;
cloud.push_back(pt);
const std::vector<int> cameraIds(1, 42);
const std::vector<float> intensities(1, 12.f);
const std::string path = tempLasPath("las");
EXPECT_EQ(saveLASFile(path, cloud, cameraIds, intensities), 0);
std::ifstream ifs(path.c_str(), std::ios::in | std::ios::binary);
liblas::ReaderFactory factory;
liblas::Reader reader = factory.CreateWithStream(ifs);
ASSERT_TRUE(reader.ReadNextPoint());
const liblas::Point & lasPt = reader.GetPoint();
EXPECT_EQ(lasPt.GetPointSourceID(), 42u);
EXPECT_NEAR(lasPt.GetIntensity(), 12.0, 1e-3);
EXPECT_NEAR(lasPt.GetColor().GetRed() / 65535.0, 255.0 / 255.0, 0.01);
EXPECT_NEAR(lasPt.GetColor().GetGreen() / 65535.0, 128.0 / 255.0, 0.01);
EXPECT_NEAR(lasPt.GetColor().GetBlue() / 65535.0, 64.0 / 255.0, 0.01);
UFile::erase(path);
}
TEST(LASWriterTest, SaveXYZICloudWithCameraIds)
{
pcl::PointCloud<pcl::PointXYZI> cloud;
pcl::PointXYZI pt;
pt.x = 1.f;
pt.y = 2.f;
pt.z = 3.f;
pt.intensity = 99.f;
cloud.push_back(pt);
const std::vector<int> cameraIds(1, 7);
const std::string path = tempLasPath("las");
EXPECT_EQ(saveLASFile(path, cloud, cameraIds), 0);
std::ifstream ifs(path.c_str(), std::ios::in | std::ios::binary);
liblas::ReaderFactory factory;
liblas::Reader reader = factory.CreateWithStream(ifs);
ASSERT_TRUE(reader.ReadNextPoint());
EXPECT_NEAR(reader.GetPoint().GetX(), 1.0, 1e-3);
EXPECT_NEAR(reader.GetPoint().GetY(), 2.0, 1e-3);
EXPECT_NEAR(reader.GetPoint().GetZ(), 3.0, 1e-3);
EXPECT_NEAR(reader.GetPoint().GetIntensity(), 99.0, 1e-3);
EXPECT_EQ(reader.GetPoint().GetPointSourceID(), 7u);
UFile::erase(path);
}
#endif // RTABMAP_LIBLAS