Skip to content

Commit ee64684

Browse files
jlblancocjlblancoc
committed
Implement InsertionContext
1 parent e5bfa10 commit ee64684

20 files changed

+1717
-623
lines changed

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

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -56,10 +56,6 @@ class CColouredPointsMap : public CPointsMap
5656
void resize(size_t newLength) override; // See base class docs
5757
void setSize(size_t newLength) override; // See base class docs
5858

59-
/** The virtual method for \a insertPoint() *without* calling
60-
* mark_as_modified() */
61-
void insertPointFast(float x, float y, float z = 0) override;
62-
6359
/** Get all the data fields for one point as a vector: [X Y Z R G B]
6460
* Unlike getPointAllFields(), this method does not check for index out of
6561
* bounds

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

Lines changed: 6 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414
#include <mrpt/opengl/pointcloud_adapters.h>
1515
#include <mrpt/serialization/CSerializable.h>
1616

17-
#include <map>
17+
#include <unordered_map>
1818

1919
namespace mrpt::maps
2020
{
@@ -61,12 +61,13 @@ class CGenericPointsMap : public CPointsMap
6161
bool unregisterField(const std::string_view& fieldName);
6262

6363
/** Returns the map of float fields: map<field_name, vector_of_data> */
64-
const std::map<std::string_view, mrpt::aligned_std_vector<float>>& float_fields() const
64+
const std::unordered_map<std::string_view, mrpt::aligned_std_vector<float>>& float_fields() const
6565
{
6666
return m_float_fields;
6767
}
6868
/** Returns the map of uint16_t fields: map<field_name, vector_of_data> */
69-
const std::map<std::string_view, mrpt::aligned_std_vector<uint16_t>>& uint16_fields() const
69+
const std::unordered_map<std::string_view, mrpt::aligned_std_vector<uint16_t>>& uint16_fields()
70+
const
7071
{
7172
return m_uint16_fields;
7273
}
@@ -80,12 +81,6 @@ class CGenericPointsMap : public CPointsMap
8081
void resize(size_t newLength) override;
8182
void setSize(size_t newLength) override;
8283

83-
/** Inserts a new point (X,Y,Z).
84-
* You **must** call `insertPointField_float()` or `insertPointField_uint16()`
85-
* *after* this for each registered field to keep data vectors synchronized.
86-
*/
87-
void insertPointFast(float x, float y, float z = 0) override;
88-
8984
void getPointAllFieldsFast(size_t index, std::vector<float>& point_data) const override;
9085
void setPointAllFieldsFast(size_t index, const std::vector<float>& point_data) override;
9186

@@ -190,9 +185,9 @@ class CGenericPointsMap : public CPointsMap
190185

191186
protected:
192187
/** Map from field name to data vector */
193-
std::map<std::string_view, mrpt::aligned_std_vector<float>> m_float_fields;
188+
std::unordered_map<std::string_view, mrpt::aligned_std_vector<float>> m_float_fields;
194189
/** Map from field name to data vector */
195-
std::map<std::string_view, mrpt::aligned_std_vector<uint16_t>> m_uint16_fields;
190+
std::unordered_map<std::string_view, mrpt::aligned_std_vector<uint16_t>> m_uint16_fields;
196191

197192
/** Clear the map, erasing all the points and all fields */
198193
void internal_clear() override;

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

Lines changed: 55 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -171,9 +171,16 @@ class CPointsMap :
171171
m_z[index] = z;
172172
}
173173

174-
/** The virtual method for \a insertPoint() *without* calling
175-
* mark_as_modified() */
176-
virtual void insertPointFast(float x, float y, float z = 0) = 0;
174+
/** Low-level method for \a insertPoint() *without* calling mark_as_modified().
175+
* Note that for derived classes having more per-point fields, you must ensure adding those fields
176+
* after this to keep the length of all vectors consistent.
177+
*/
178+
void insertPointFast(float x, float y, float z)
179+
{
180+
m_x.push_back(x);
181+
m_y.push_back(y);
182+
m_z.push_back(z);
183+
}
177184

178185
/** Get all the data fields for one point as a vector: depending on the
179186
* implementation class this can be [X Y Z] or [X Y Z R G B], etc...
@@ -200,9 +207,10 @@ class CPointsMap :
200207
this->clear();
201208
this->reserve(N);
202209
this->registerPointFieldsFrom(obj);
210+
const auto ctx = this->prepareForInsertPointsFrom(obj);
203211
for (size_t i = 0; i < N; i++)
204212
{
205-
insertPointFrom(obj, i);
213+
insertPointFrom(obj, i, ctx);
206214
}
207215
}
208216

@@ -798,28 +806,56 @@ class CPointsMap :
798806
return allAdded;
799807
}
800808

809+
/** Insert context for insertPointFrom().
810+
* \sa prepareForInsertPointsFrom()
811+
*/
812+
struct InsertCtx
813+
{
814+
// For XYZ
815+
const mrpt::aligned_std_vector<float>* xs_src = nullptr;
816+
const mrpt::aligned_std_vector<float>* ys_src = nullptr;
817+
const mrpt::aligned_std_vector<float>* zs_src = nullptr;
818+
819+
// Optional field mappings (only fields present in both maps)
820+
struct FloatFieldMapping
821+
{
822+
const mrpt::aligned_std_vector<float>* src_buf = nullptr;
823+
mrpt::aligned_std_vector<float>* dst_buf = nullptr;
824+
};
825+
std::vector<FloatFieldMapping> float_fields;
826+
827+
struct UInt16FieldMapping
828+
{
829+
const mrpt::aligned_std_vector<uint16_t>* src_buf = nullptr;
830+
mrpt::aligned_std_vector<uint16_t>* dst_buf = nullptr;
831+
};
832+
std::vector<UInt16FieldMapping> uint16_fields;
833+
};
834+
835+
/** Prepare efficient data structures for repeated insertion from another point map with
836+
* insertPointFrom() */
837+
InsertCtx prepareForInsertPointsFrom(const CPointsMap& source) const;
838+
801839
/** Generic method to copy *all* applicable point properties from
802840
* one map to another, e.g. timestamp, intensity, etc.
803841
* \note Before calling this in a loop, make sure of calling registerPointFieldsFrom()
804842
*/
805-
void insertPointFrom(const mrpt::maps::CPointsMap& source, size_t sourcePointIndex)
843+
void insertPointFrom(const CPointsMap& source, size_t sourcePointIndex, const InsertCtx& ctx)
806844
{
807-
const auto i = sourcePointIndex; // shortcut
808-
// mandatory fields: XYZ
809-
const auto& xs = source.getPointsBufferRef_x();
810-
const auto& ys = source.getPointsBufferRef_y();
811-
const auto& zs = source.getPointsBufferRef_z();
812-
insertPointFast(xs[i], ys[i], zs[i]);
813-
814-
// Optional fields: only if they already exist on both maps:
815-
for (const auto& f : this->getPointFieldNames_float_except_xyz())
845+
const size_t i = sourcePointIndex;
846+
insertPointFast((*ctx.xs_src)[i], (*ctx.ys_src)[i], (*ctx.zs_src)[i]);
847+
848+
// Optional fields
849+
for (auto& f : ctx.float_fields)
816850
{
817-
insertPointField_float(f, source.getPointField_float(i, f));
851+
f.dst_buf->push_back((*f.src_buf)[i]);
818852
}
819-
for (const auto& f : this->getPointFieldNames_uint16())
853+
854+
for (auto& f : ctx.uint16_fields)
820855
{
821-
insertPointField_uint16(f, source.getPointField_uint16(i, f));
856+
f.dst_buf->push_back((*f.src_buf)[i]);
822857
}
858+
823859
mark_as_modified();
824860
}
825861

@@ -1171,7 +1207,9 @@ class CPointsMap :
11711207
clear();
11721208
reserve(N);
11731209
for (size_t i = 0; i < N; ++i)
1210+
{
11741211
this->insertPointFast(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z);
1212+
}
11751213
}
11761214

11771215
/** @} */

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

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -49,10 +49,6 @@ class CPointsMapXYZI : public CPointsMap
4949
void resize(size_t newLength) override; // See base class docs
5050
void setSize(size_t newLength) override; // See base class docs
5151

52-
/** The virtual method for \a insertPoint() *without* calling
53-
* mark_as_modified() */
54-
void insertPointFast(float x, float y, float z = 0) override;
55-
5652
/** Get all the data fields for one point as a vector: [X Y Z I]
5753
* Unlike getPointAllFields(), this method does not check for index out of
5854
* bounds

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

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -65,10 +65,6 @@ class CPointsMapXYZIRT : public CPointsMap
6565
bool hasRingField() const { return !m_ring.empty(); }
6666
bool hasTimeField() const { return !m_time.empty(); }
6767

68-
/** The virtual method for \a insertPoint() *without* calling
69-
* mark_as_modified() */
70-
void insertPointFast(float x, float y, float z = 0) override;
71-
7268
/** Get all the data fields for one point as a vector: [X Y Z I]
7369
* Unlike getPointAllFields(), this method does not check for index out of
7470
* bounds

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

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -54,9 +54,7 @@ class CSimplePointsMap : public CPointsMap
5454
void reserve(size_t newLength) override; // See base class docs
5555
void resize(size_t newLength) override; // See base class docs
5656
void setSize(size_t newLength) override; // See base class docs
57-
/** The virtual method for \a insertPoint() *without* calling
58-
* mark_as_modified() */
59-
void insertPointFast(float x, float y, float z = 0) override;
57+
6058
/** Get all the data fields for one point as a vector: [X Y Z]
6159
* Unlike getPointAllFields(), this method does not check for index out of
6260
* bounds

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

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -54,10 +54,6 @@ class CWeightedPointsMap : public CPointsMap
5454
void resize(size_t newLength) override; // See base class docs
5555
void setSize(size_t newLength) override; // See base class docs
5656

57-
/** The virtual method for \a insertPoint() *without* calling
58-
* mark_as_modified() */
59-
void insertPointFast(float x, float y, float z = 0) override;
60-
6157
/** Get all the data fields for one point as a vector: [X Y Z WEIGHT]
6258
* Unlike getPointAllFields(), this method does not check for index out of
6359
* bounds

‎libs/maps/src/maps/CColouredPointsMap.cpp‎

Lines changed: 0 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -317,14 +317,6 @@ void CColouredPointsMap::setPointColor(size_t index, float R, float G, float B)
317317
// mark_as_modified(); // No need to rebuild KD-trees, etc...
318318
}
319319

320-
void CColouredPointsMap::insertPointFast(float x, float y, float z)
321-
{
322-
m_x.push_back(x);
323-
m_y.push_back(y);
324-
m_z.push_back(z);
325-
// mark_as_modified(); -> Fast
326-
}
327-
328320
void CColouredPointsMap::insertPointRGB(float x, float y, float z, float R, float G, float B)
329321
{
330322
m_x.push_back(x);

‎libs/maps/src/maps/CGenericPointsMap.cpp‎

Lines changed: 0 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -237,15 +237,6 @@ bool CGenericPointsMap::unregisterField(const std::string_view& fieldName)
237237
return false;
238238
}
239239

240-
void CGenericPointsMap::insertPointFast(float x, float y, float z)
241-
{
242-
m_x.push_back(x);
243-
m_y.push_back(y);
244-
m_z.push_back(z);
245-
// NOTE: This intentionally makes field vectors one element shorter
246-
// than m_x,y,z. User MUST call insertPointField_...() to fix this.
247-
}
248-
249240
void CGenericPointsMap::getPointAllFieldsFast(size_t index, std::vector<float>& point_data) const
250241
{
251242
const size_t nFields = 3 + m_float_fields.size() + m_uint16_fields.size();

‎libs/maps/src/maps/CPointsMap.cpp‎

Lines changed: 45 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1523,6 +1523,8 @@ void CPointsMap::insertAnotherMap(
15231523
// matrix multiplications:
15241524
const bool identity_tf = (otherPose == CPose3D::Identity());
15251525

1526+
const auto ctx = this->prepareForInsertPointsFrom(*otherMap);
1527+
15261528
mrpt::math::TPoint3Df pt;
15271529
for (size_t srcIdx = 0; srcIdx < N_other; srcIdx++)
15281530
{
@@ -1537,7 +1539,7 @@ void CPointsMap::insertAnotherMap(
15371539
if (pt.x != pt.x) continue;
15381540

15391541
// Add to this map:
1540-
this->insertPointFrom(*otherMap, srcIdx);
1542+
this->insertPointFrom(*otherMap, srcIdx, ctx);
15411543

15421544
// and overwrite the XYZ, if needed:
15431545
if (!identity_tf)
@@ -2051,6 +2053,48 @@ std::vector<std::string_view> CPointsMap::getPointFieldNames_float_except_xyz()
20512053
return result;
20522054
}
20532055

2056+
CPointsMap::InsertCtx CPointsMap::prepareForInsertPointsFrom(const CPointsMap& source) const
2057+
{
2058+
InsertCtx ctx;
2059+
2060+
// Mandatory XYZ
2061+
ctx.xs_src = &source.getPointsBufferRef_x();
2062+
ctx.ys_src = &source.getPointsBufferRef_y();
2063+
ctx.zs_src = &source.getPointsBufferRef_z();
2064+
2065+
// Optional fields: match by name but store direct pointers
2066+
const auto src_float_names = source.getPointFieldNames_float_except_xyz();
2067+
const auto dst_float_names = this->getPointFieldNames_float_except_xyz();
2068+
2069+
for (const auto& name : dst_float_names)
2070+
{
2071+
if (auto it = std::find(src_float_names.begin(), src_float_names.end(), name);
2072+
it != src_float_names.end())
2073+
{
2074+
ctx.float_fields.push_back(
2075+
{source.getPointsBufferRef_float_field(name),
2076+
const_cast<mrpt::aligned_std_vector<float>*>(
2077+
this->getPointsBufferRef_float_field(name))});
2078+
}
2079+
}
2080+
2081+
const auto src_u16_names = source.getPointFieldNames_uint16();
2082+
const auto dst_u16_names = this->getPointFieldNames_uint16();
2083+
for (const auto& name : dst_u16_names)
2084+
{
2085+
if (auto it = std::find(src_u16_names.begin(), src_u16_names.end(), name);
2086+
it != src_u16_names.end())
2087+
{
2088+
ctx.uint16_fields.push_back(
2089+
{source.getPointsBufferRef_uint_field(name),
2090+
const_cast<mrpt::aligned_std_vector<uint16_t>*>(
2091+
this->getPointsBufferRef_uint_field(name))});
2092+
}
2093+
}
2094+
2095+
return ctx;
2096+
}
2097+
20542098
// =========== API of the NearestNeighborsCapable virtual interface ======
20552099
// See docs in base class
20562100
void CPointsMap::nn_prepare_for_2d_queries() const

0 commit comments

Comments
 (0)