Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 8 additions & 2 deletions agents.md
Original file line number Diff line number Diff line change
Expand Up @@ -283,6 +283,13 @@ Classes registered by `src/register.cpp` (these are the names a YAML must use):
`mola::SparseVoxelPointCloud`, `mola::SparseTreesPointCloud`, `mola::NDT`,
`mola::TSDF`, `mola::KeyframePointCloudMap`, `mola::IncrementalPointCloud`.

- `insertObservation()` follows MRPT: points of point clouds, Velodyne scans and
`points3D_*` are in the **sensor** frame, placed at `robotPose + sensorPose`;
TSDF/NDT use the sensor position as ray origin. `KeyframePointCloudMap` keeps
KF poses as robot poses and moves the cloud to the robot frame instead.
Pipelines (`FilterMerge`) insert robot-frame clouds with an identity `sensorPose`.
Tests: `tests/test-sensor-pose-insertion.cpp`.

- `TSDF` (new 2026): voxel-hashed truncated signed distance field. Each measured
point writes a signed distance **along its own ray** into the voxels of a
narrow cylinder around it (`ray_tube_voxels` is the coverage knob,
Expand Down Expand Up @@ -639,8 +646,7 @@ Classes registered by `src/register.cpp` (these are the names a YAML must use):
`trySetCreationOptions(const TCreationOptions&)` (raises if refused). The other option
groups are live references. `ViewDirectionFilter.None` is `None_` in Python.
- `insertPointCloud()` builds a `CObservationPointCloud` with the points in the **robot**
frame and an identity `sensorPose`, which most classes here ignore anyway (TSDF/NDT use
the robot position as ray origin).
frame and an identity `sensorPose`, so TSDF/NDT use the robot position as ray origin.
- After changing bindings, regenerate `mola_metric_maps/_core.pyi` with
`scripts/generate_stubs.py`.
- pip wheel `mola-metric-maps` (`packaging/pip/`, read its `README-dev.md`): a
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,10 @@ namespace mola
*
* ## Input
* The caller inserts `mrpt::obs::CObservationPointCloud` observations, with
* points already expressed in the vehicle `base_link` frame and already motion
* compensated (deskewed). This is typically produced by an `mp2p_icp::Generator`
* plus optional filter pipeline.
* points already motion compensated (deskewed). This is typically produced by
* an `mp2p_icp::Generator` plus optional filter pipeline. As in MRPT, points are
* in the sensor frame, placed on the vehicle by the observation `sensorPose`;
* keyframes store them in the vehicle `base_link` frame.
*
* ## Per-keyframe data
* Each `KeyFrame` holds:
Expand Down
9 changes: 5 additions & 4 deletions mola_metric_maps/include/mola_metric_maps/NDT.h
Original file line number Diff line number Diff line change
Expand Up @@ -612,12 +612,13 @@ class NDT : public mrpt::maps::CMetricMap,
const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, const float* zs,
const std::size_t num_pts) const;

/** - (xs,ys,zs): Sensed point local coordinates in the robot frame.
* - pc_in_map: SE(3) pose of the robot in the map frame.
/** - (xs,ys,zs): Sensed point local coordinates, in the frame given by pc_in_map.
* - pc_in_map: SE(3) pose of the frame of (xs,ys,zs) in the map frame.
* - sensorPt: sensor position in the map frame, the origin of the rays.
*/
void internal_insertPointCloud3D(
const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, const float* zs,
const std::size_t num_pts);
const mrpt::poses::CPose3D& pc_in_map, const mrpt::math::TPoint3Df& sensorPt, const float* xs,
const float* ys, const float* zs, const std::size_t num_pts);

// See docs in base class
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation& obs) const override;
Expand Down
9 changes: 5 additions & 4 deletions mola_metric_maps/include/mola_metric_maps/TSDF.h
Original file line number Diff line number Diff line change
Expand Up @@ -524,12 +524,13 @@ class TSDF : public mrpt::maps::CMetricMap,
/** Removes voxels far from the given sensor position. */
void pruneFarVoxels(const mrpt::math::TPoint3Df& sensorPt);

/** - (xs,ys,zs): sensed point coordinates in the sensor frame.
* - pc_in_map: SE(3) pose of the sensor in the map frame.
/** - (xs,ys,zs): sensed point coordinates, in the frame given by pc_in_map.
* - pc_in_map: SE(3) pose of the frame of (xs,ys,zs) in the map frame.
* - sensorPt: sensor position in the map frame, the origin of the rays.
*/
void internal_insertPointCloud3D(
const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, const float* zs,
const std::size_t num_pts);
const mrpt::poses::CPose3D& pc_in_map, const mrpt::math::TPoint3Df& sensorPt, const float* xs,
const float* ys, const float* zs, const std::size_t num_pts);
};

} // namespace mola
14 changes: 11 additions & 3 deletions mola_metric_maps/src/HashedVoxelPointCloud.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -381,9 +381,13 @@ bool HashedVoxelPointCloud::internal_insertObservation(
// Empty point set, or load from XYZ in observation:
if (o.hasPoints3D)
{
// points3D_* are in the sensor frame:
const auto sensorPose3D = robotPose3D + o.sensorPose;
for (size_t i = 0; i < o.points3D_x.size(); i++)
{
this->insertPoint(
robotPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]}));
sensorPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]}));
}

return true;
}
Expand Down Expand Up @@ -413,10 +417,12 @@ bool HashedVoxelPointCloud::internal_insertObservation(
// Automatically generate pointcloud if needed:
if (!o.point_cloud.size()) const_cast<CObservationVelodyneScan&>(o).generatePointCloud();

// point_cloud is in the sensor frame:
const auto sensorPose3D = robotPose3D + o.sensorPose;
for (size_t i = 0; i < o.point_cloud.x.size(); i++)
{
insertPoint(
robotPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]}));
sensorPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]}));
}

return true;
Expand All @@ -430,9 +436,11 @@ bool HashedVoxelPointCloud::internal_insertObservation(
const auto& ys = o.pointcloud->getPointsBufferRef_y();
const auto& zs = o.pointcloud->getPointsBufferRef_z();

// The cloud is in the sensor frame:
const auto sensorPose3D = robotPose3D + o.sensorPose;
for (size_t i = 0; i < xs.size(); i++)
{
insertPoint(robotPose3D.composePoint({xs[i], ys[i], zs[i]}));
insertPoint(sensorPose3D.composePoint({xs[i], ys[i], zs[i]}));
}

return true;
Expand Down
29 changes: 28 additions & 1 deletion mola_metric_maps/src/KeyframePointCloudMap.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,33 @@ void rotateViewDirectionFieldsOrFallback(Pts& pts, const Pose& tf)
}
}
#endif

/** The cloud of `obs` in the robot frame. Keyframes keep their points in the
* robot frame, while the observation holds them in the sensor frame, so a
* transformed copy is returned if the sensor is not at the robot origin. */
mrpt::maps::CPointsMap::Ptr cloudInRobotFrame(const mrpt::obs::CObservationPointCloud& obs)
{
if (obs.sensorPose == mrpt::poses::CPose3D::Identity())
{
return obs.pointcloud;
}

// Same class as the input, to keep all its point fields:
auto out = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(
obs.pointcloud->GetRuntimeClass()->createObject());
ASSERT_(out);
out->insertAnotherMap(obs.pointcloud.get(), obs.sensorPose);

// Before MRPT 3.6.0, insertAnotherMap() copies view_{x,y,z} verbatim:
#if MRPT_VERSION < 0x030600
#if defined(MOLA_MM_HAS_ROTATE_VIEW_HEADER)
rotateViewDirectionFieldsOrFallback(*out, obs.sensorPose);
#else
rotateViewDirectionFieldsLegacy(*out, obs.sensorPose);
#endif
#endif
return out;
}
} // namespace

const thread_local auto ENV_DO_PROFILE_COV =
Expand Down Expand Up @@ -2849,7 +2876,7 @@ bool KeyframePointCloudMap::internal_insertObservation(

new_kf.timestamp = obs.timestamp;
new_kf.pose(pc_in_map);
new_kf.pointcloud(obsPC->pointcloud);
new_kf.pointcloud(cloudInRobotFrame(*obsPC));

new_kf.buildCache();
cached_.reset();
Expand Down
31 changes: 20 additions & 11 deletions mola_metric_maps/src/NDT.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -440,7 +440,10 @@ bool NDT::internal_insertObservation(
{
// Default values are (0,0,0)
}
const auto sensorPt = robotPose3D.translation().cast<float>();

// Rays start at the sensor, not at the robot origin:
const auto sensorPose3D = robotPose3D + obs.getSensorPose();
const auto sensorPt = sensorPose3D.translation().cast<float>();

if (insertionOptions.remove_voxels_farther_than > 0)
{
Expand Down Expand Up @@ -487,7 +490,8 @@ bool NDT::internal_insertObservation(
const auto& ys = scanPoints->getPointsBufferRef_y();
const auto& zs = scanPoints->getPointsBufferRef_z();

internal_insertPointCloud3D(robotPose3D, xs.data(), ys.data(), zs.data(), xs.size());
// Points are already in the robot frame:
internal_insertPointCloud3D(robotPose3D, sensorPt, xs.data(), ys.data(), zs.data(), xs.size());

return true;
}
Expand All @@ -505,10 +509,13 @@ bool NDT::internal_insertObservation(
// Empty point set, or load from XYZ in observation:
if (o.hasPoints3D)
{
// points3D_* are in the sensor frame:
for (size_t i = 0; i < o.points3D_x.size(); i++)
{
this->insertPoint(
robotPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]}),
sensorPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]}),
sensorPt);
}

return true;
}
Expand All @@ -522,7 +529,9 @@ bool NDT::internal_insertObservation(
const auto& ys = pointMap.getPointsBufferRef_y();
const auto& zs = pointMap.getPointsBufferRef_z();

internal_insertPointCloud3D(robotPose3D, xs.data(), ys.data(), zs.data(), xs.size());
// unprojectInto() already brings the points into the robot frame:
internal_insertPointCloud3D(
robotPose3D, sensorPt, xs.data(), ys.data(), zs.data(), xs.size());

return true;
}
Expand All @@ -543,10 +552,11 @@ bool NDT::internal_insertObservation(
const_cast<CObservationVelodyneScan&>(o).generatePointCloud();
}

// point_cloud is in the sensor frame:
for (size_t i = 0; i < o.point_cloud.x.size(); i++)
{
insertPoint(
robotPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]}),
sensorPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]}),
sensorPt);
}

Expand All @@ -562,9 +572,10 @@ bool NDT::internal_insertObservation(
const auto& ys = o.pointcloud->getPointsBufferRef_y();
const auto& zs = o.pointcloud->getPointsBufferRef_z();

// The cloud is in the sensor frame:
for (size_t i = 0; i < xs.size(); i++)
{
insertPoint(robotPose3D.composePoint({xs[i], ys[i], zs[i]}), sensorPt);
insertPoint(sensorPose3D.composePoint({xs[i], ys[i], zs[i]}), sensorPt);
}

return true;
Expand Down Expand Up @@ -1065,21 +1076,19 @@ void NDT::TRenderOptions::loadFromConfigFile(
}

void NDT::internal_insertPointCloud3D(
const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, const float* zs,
const std::size_t num_pts)
const mrpt::poses::CPose3D& pc_in_map, const mrpt::math::TPoint3Df& sensorPt, const float* xs,
const float* ys, const float* zs, const std::size_t num_pts)
{
MRPT_TRY_START

voxels_.reserve(voxels_.size() + num_pts);

const auto sensorPose = pc_in_map.translation().cast<float>();

for (std::size_t i = 0; i < num_pts; i++)
{
// Transform the point from the scan reference to its global 3D
// position:
const auto gPt = pc_in_map.composePoint({xs[i], ys[i], zs[i]});
insertPoint(gPt, sensorPose);
insertPoint(gPt, sensorPt);
}

MRPT_TRY_END
Expand Down
13 changes: 9 additions & 4 deletions mola_metric_maps/src/SparseTreesPointCloud.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -365,10 +365,12 @@ bool SparseTreesPointCloud::internal_insertObservation(
// Empty point set, or load from XYZ in observation:
if (o.hasPoints3D)
{
// points3D_* are in the sensor frame:
const auto sensorPose3D = robotPose3D + o.sensorPose;
for (size_t i = 0; i < o.points3D_x.size(); i++)
{
this->insertPoint(
robotPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]}));
sensorPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]}));
}

return true;
Expand Down Expand Up @@ -404,9 +406,10 @@ bool SparseTreesPointCloud::internal_insertObservation(
const_cast<CObservationVelodyneScan&>(o).generatePointCloud();
}

// point_cloud is in the sensor frame:
internal_insertPointCloud3D(
robotPose3D, o.point_cloud.x.data(), o.point_cloud.y.data(), o.point_cloud.z.data(),
o.point_cloud.x.size());
robotPose3D + o.sensorPose, o.point_cloud.x.data(), o.point_cloud.y.data(),
o.point_cloud.z.data(), o.point_cloud.x.size());

return true;
}
Expand All @@ -420,7 +423,9 @@ bool SparseTreesPointCloud::internal_insertObservation(
const auto& ys = o.pointcloud->getPointsBufferRef_y();
const auto& zs = o.pointcloud->getPointsBufferRef_z();

internal_insertPointCloud3D(robotPose3D, xs.data(), ys.data(), zs.data(), xs.size());
// The cloud is in the sensor frame:
internal_insertPointCloud3D(
robotPose3D + o.sensorPose, xs.data(), ys.data(), zs.data(), xs.size());

return true;
}
Expand Down
14 changes: 11 additions & 3 deletions mola_metric_maps/src/SparseVoxelPointCloud.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -411,9 +411,13 @@ bool SparseVoxelPointCloud::internal_insertObservation(
// Empty point set, or load from XYZ in observation:
if (o.hasPoints3D)
{
// points3D_* are in the sensor frame:
const auto sensorPose3D = robotPose3D + o.sensorPose;
for (size_t i = 0; i < o.points3D_x.size(); i++)
{
this->insertPoint(
robotPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]}));
sensorPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]}));
}

return true;
}
Expand Down Expand Up @@ -443,10 +447,12 @@ bool SparseVoxelPointCloud::internal_insertObservation(
// Automatically generate pointcloud if needed:
if (!o.point_cloud.size()) const_cast<CObservationVelodyneScan&>(o).generatePointCloud();

// point_cloud is in the sensor frame:
const auto sensorPose3D = robotPose3D + o.sensorPose;
for (size_t i = 0; i < o.point_cloud.x.size(); i++)
{
insertPoint(
robotPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]}));
sensorPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]}));
}

return true;
Expand All @@ -460,9 +466,11 @@ bool SparseVoxelPointCloud::internal_insertObservation(
const auto& ys = o.pointcloud->getPointsBufferRef_y();
const auto& zs = o.pointcloud->getPointsBufferRef_z();

// The cloud is in the sensor frame:
const auto sensorPose3D = robotPose3D + o.sensorPose;
for (size_t i = 0; i < xs.size(); i++)
{
insertPoint(robotPose3D.composePoint({xs[i], ys[i], zs[i]}));
insertPoint(sensorPose3D.composePoint({xs[i], ys[i], zs[i]}));
}

return true;
Expand Down
Loading
Loading