Skip to content

Commit b5dbf10

Browse files
jlblancocjlblancoc
committed
WIP: Add new Voxelmap classes
1 parent 88d61cf commit b5dbf10

File tree

10 files changed

+461
-1
lines changed

10 files changed

+461
-1
lines changed

‎doc/source/doxygen-docs/changelog.md

+6-1
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,11 @@
11
\page changelog Change Log
22

3-
# Version 2.10.3: UNRELEASED
3+
# Version 2.11.0: UNRELEASED
4+
- Changes in libraries:
5+
- \ref mrpt_maps_grp
6+
- New voxel map containers, based on Faconti's [Bonxai](https://github.com/facontidavide/Bonxai) header-only libray (MPL-2.0 license):
7+
- mrpt::maps::CVoxelMap
8+
- mrpt::maps::CVoxelMapRGB
49
- BUG FIXES:
510
- Fix python wrapper FTBFS in armhf and other architectures.
611
- Fix matrices removeColumns() and removeRows() won't throw if user specified a non-existing index.

‎doc/source/doxygen-docs/example-maps_voxelmap_simple.md

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
\page maps_octomap_simple Example: maps_octomap_simple
2+
3+
![maps_voxelmap_simple screenshot](doc/source/images/maps_voxelmap_simple_screenshot.png)
4+
C++ example source code:
5+
\include maps_voxelmap_simple/test.cpp

‎doc/source/examples.rst

+1
Original file line numberDiff line numberDiff line change
@@ -78,6 +78,7 @@ Python examples are `here <python_examples.html>`_.
7878
page_maps_observer_pattern_example.rst
7979
page_maps_octomap_simple.rst
8080
page_maps_ransac_data_association.rst
81+
page_maps_voxelmap_simple.rst
8182
page_math_csparse_example.rst
8283
page_math_kmeans_example.rst
8384
page_math_leastsquares_example.rst

‎libs/maps/include/mrpt/maps/CVoxelMap.h

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
/* +------------------------------------------------------------------------+
2+
| Mobile Robot Programming Toolkit (MRPT) |
3+
| https://www.mrpt.org/ |
4+
| |
5+
| Copyright (c) 2005-2023, Individual contributors, see AUTHORS file |
6+
| See: https://www.mrpt.org/Authors - All rights reserved. |
7+
| Released under BSD License. See: https://www.mrpt.org/License |
8+
+------------------------------------------------------------------------+ */
9+
10+
#pragma once
11+
12+
#include <mrpt/config/CLoadableOptions.h>
13+
#include <mrpt/core/safe_pointers.h>
14+
#include <mrpt/maps/CMetricMap.h>
15+
#include <mrpt/maps/COctoMapBase.h>
16+
#include <mrpt/obs/obs_frwds.h>
17+
18+
namespace octomap
19+
{
20+
class OcTree;
21+
}
22+
namespace octomap
23+
{
24+
class OcTreeNode;
25+
}
26+
27+
namespace mrpt::maps
28+
{
29+
}

‎libs/maps/include/mrpt/maps/CVoxelMapBase.h

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
/* +------------------------------------------------------------------------+
2+
| Mobile Robot Programming Toolkit (MRPT) |
3+
| https://www.mrpt.org/ |
4+
| |
5+
| Copyright (c) 2005-2023, Individual contributors, see AUTHORS file |
6+
| See: https://www.mrpt.org/Authors - All rights reserved. |
7+
| Released under BSD License. See: https://www.mrpt.org/License |
8+
+------------------------------------------------------------------------+ */
9+
10+
#pragma once
11+
12+
#include <mrpt/config/CLoadableOptions.h>
13+
#include <mrpt/core/safe_pointers.h>
14+
#include <mrpt/maps/CMetricMap.h>
15+
#include <mrpt/maps/COctoMapBase.h>
16+
#include <mrpt/obs/obs_frwds.h>
17+
18+
namespace octomap
19+
{
20+
class OcTree;
21+
}
22+
namespace octomap
23+
{
24+
class OcTreeNode;
25+
}
26+
27+
namespace mrpt::maps
28+
{
29+
}

‎libs/maps/include/mrpt/maps/CVoxelMapRGB.h

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
/* +------------------------------------------------------------------------+
2+
| Mobile Robot Programming Toolkit (MRPT) |
3+
| https://www.mrpt.org/ |
4+
| |
5+
| Copyright (c) 2005-2023, Individual contributors, see AUTHORS file |
6+
| See: https://www.mrpt.org/Authors - All rights reserved. |
7+
| Released under BSD License. See: https://www.mrpt.org/License |
8+
+------------------------------------------------------------------------+ */
9+
10+
#pragma once
11+
12+
#include <mrpt/config/CLoadableOptions.h>
13+
#include <mrpt/core/safe_pointers.h>
14+
#include <mrpt/maps/CMetricMap.h>
15+
#include <mrpt/maps/COctoMapBase.h>
16+
#include <mrpt/obs/obs_frwds.h>
17+
18+
namespace octomap
19+
{
20+
class OcTree;
21+
}
22+
namespace octomap
23+
{
24+
class OcTreeNode;
25+
}
26+
27+
namespace mrpt::maps
28+
{
29+
}

‎libs/maps/src/maps/CVoxelMap.cpp

+109
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,109 @@
1+
/* +------------------------------------------------------------------------+
2+
| Mobile Robot Programming Toolkit (MRPT) |
3+
| https://www.mrpt.org/ |
4+
| |
5+
| Copyright (c) 2005-2023, Individual contributors, see AUTHORS file |
6+
| See: https://www.mrpt.org/Authors - All rights reserved. |
7+
| Released under BSD License. See: https://www.mrpt.org/License |
8+
+------------------------------------------------------------------------+ */
9+
10+
#include "maps-precomp.h" // Precomp header
11+
//
12+
#include <mrpt/maps/CVoxelMap.h>
13+
14+
using namespace mrpt::maps;
15+
16+
// =========== Begin of Map definition ============
17+
MAP_DEFINITION_REGISTER("mrpt::maps::CVoxelMap", mrpt::maps::CVoxelMap)
18+
19+
CVoxelMap::TMapDefinition::TMapDefinition() = default;
20+
void CVoxelMap::TMapDefinition::loadFromConfigFile_map_specific(
21+
const mrpt::config::CConfigFileBase& source,
22+
const std::string& sectionNamePrefix)
23+
{
24+
// [<sectionNamePrefix>+"_creationOpts"]
25+
const std::string sSectCreation =
26+
sectionNamePrefix + string("_creationOpts");
27+
MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
28+
29+
insertionOpts.loadFromConfigFile(
30+
source, sectionNamePrefix + string("_insertOpts"));
31+
likelihoodOpts.loadFromConfigFile(
32+
source, sectionNamePrefix + string("_likelihoodOpts"));
33+
}
34+
35+
void CVoxelMap::TMapDefinition::dumpToTextStream_map_specific(
36+
std::ostream& out) const
37+
{
38+
LOADABLEOPTS_DUMP_VAR(resolution, double);
39+
40+
this->insertionOpts.dumpToTextStream(out);
41+
this->likelihoodOpts.dumpToTextStream(out);
42+
}
43+
44+
mrpt::maps::CMetricMap* CVoxelMap::internal_CreateFromMapDefinition(
45+
const mrpt::maps::TMetricMapInitializer& _def)
46+
{
47+
const CVoxelMap::TMapDefinition& def =
48+
*dynamic_cast<const CVoxelMap::TMapDefinition*>(&_def);
49+
auto* obj = new CVoxelMap(def.resolution);
50+
obj->insertionOptions = def.insertionOpts;
51+
obj->likelihoodOptions = def.likelihoodOpts;
52+
return obj;
53+
}
54+
// =========== End of Map definition Block =========
55+
56+
IMPLEMENTS_SERIALIZABLE(CVoxelMap, CMetricMap, mrpt::maps)
57+
58+
/*---------------------------------------------------------------
59+
Constructor
60+
---------------------------------------------------------------*/
61+
CVoxelMap::CVoxelMap(const double resolution)
62+
: CVoxelMapBase<octomap::OcTree, octomap::OcTreeNode>(resolution)
63+
{
64+
}
65+
66+
CVoxelMap::~CVoxelMap() = default;
67+
uint8_t CVoxelMap::serializeGetVersion() const { return 0; }
68+
void CVoxelMap::serializeTo(mrpt::serialization::CArchive& out) const
69+
{
70+
this->likelihoodOptions.writeToStream(out);
71+
this->renderingOptions.writeToStream(out); // Added in v1
72+
out << genericMapParams;
73+
// const_cast<octomap::OcTree*>(&m_impl->m_octomap)->writeBinary(ss);
74+
}
75+
76+
void CVoxelMap::serializeFrom(
77+
mrpt::serialization::CArchive& in, uint8_t version)
78+
{
79+
switch (version)
80+
{
81+
case 0:
82+
{
83+
likelihoodOptions.readFromStream(in);
84+
renderingOptions.readFromStream(in);
85+
in >> genericMapParams;
86+
87+
this->clear();
88+
89+
m_impl->m_octomap.readBinary(ss);
90+
}
91+
break;
92+
default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
93+
};
94+
}
95+
96+
bool CVoxelMap::internal_insertObservation(
97+
const mrpt::obs::CObservation& obs,
98+
const std::optional<const mrpt::poses::CPose3D>& robotPose)
99+
{
100+
octomap::point3d sensorPt;
101+
octomap::Pointcloud scan;
102+
if (!internal_build_PointCloud_for_observation(
103+
obs, robotPose, sensorPt, scan))
104+
return false; // Nothing to do.
105+
// Insert rays:
106+
m_impl->m_octomap.insertPointCloud(
107+
scan, sensorPt, insertionOptions.maxrange, insertionOptions.pruning);
108+
return true;
109+
}

‎samples/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -287,6 +287,7 @@ if(MRPT_BUILD_EXAMPLES)
287287
maps_gridmap3D_simple
288288
maps_observer_pattern_example
289289
maps_octomap_simple
290+
maps_voxelmap_simple
290291
maps_gmrf_map_example
291292
maps_ransac_data_association
292293
)

‎samples/maps_voxelmap_simple/CMakeLists.txt

+62
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
#-----------------------------------------------------------------------------------------------
2+
# CMake file for the MRPT example: /maps_voxelmap_simple
3+
#
4+
# Run with "ccmake ." at the root directory, or use it as a template for
5+
# starting your own programs
6+
#-----------------------------------------------------------------------------------------------
7+
set(sampleName maps_voxelmap_simple)
8+
project(EXAMPLE_${sampleName})
9+
10+
cmake_minimum_required(VERSION 3.1)
11+
12+
# ---------------------------------------------------------------------------
13+
# Set the output directory of each example to its corresponding subdirectory
14+
# in the binary tree:
15+
# ---------------------------------------------------------------------------
16+
set(EXECUTABLE_OUTPUT_PATH ".")
17+
18+
# The list of "libs" which can be included can be found in:
19+
# https://www.mrpt.org/Libraries
20+
# Add the top-level dependencies only.
21+
# --------------------------------------------------------------------------
22+
foreach(dep maps;gui)
23+
# if not building from inside MRPT source tree, find it as a cmake
24+
# imported project:
25+
if (NOT TARGET mrpt::${dep})
26+
find_package(mrpt-${dep} REQUIRED)
27+
endif()
28+
endforeach()
29+
30+
# Define the executable target:
31+
add_executable(${sampleName} test.cpp )
32+
33+
if(TARGET examples)
34+
add_dependencies(examples ${sampleName})
35+
endif()
36+
37+
set_target_properties(
38+
${sampleName}
39+
PROPERTIES
40+
PROJECT_LABEL "(EXAMPLE) ${sampleName}")
41+
42+
# Add special defines needed by this example, if any:
43+
set(MY_DEFS )
44+
if(MY_DEFS) # If not empty
45+
add_definitions("-D${MY_DEFS}")
46+
endif()
47+
48+
# Add the required libraries for linking:
49+
foreach(dep maps;gui)
50+
target_link_libraries(${sampleName} mrpt::${dep})
51+
endforeach()
52+
53+
# Set optimized building:
54+
if((${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang" OR CMAKE_COMPILER_IS_GNUCXX) AND NOT CMAKE_BUILD_TYPE MATCHES "Debug")
55+
add_compile_options(-O3)
56+
endif()
57+
58+
# This part can be removed if you are compiling this program outside of
59+
# the MRPT tree:
60+
if(DEFINED MRPT_LIBS_ROOT) # Fails if build outside of MRPT project.
61+
DeclareAppDependencies(${sampleName} mrpt::maps;mrpt::gui) # Dependencies
62+
endif()

0 commit comments

Comments
 (0)