kennyjchen
commented
2 years ago Hi @qweesiic --
We calculate covariances twice for each acquired scan:
- In
base_linkframe ingetNextPose()by callingalign()here, which is an inheritedpcl::Registrationfunction that calls nano_gicp's templatedcomputeTransformation()(which calculates covariances if none are available) - In
odomframe inupdateKeyframes()here by explicitly calling nano_gicp'scalculateSourceCovariances()
The first is for alignment between adjacent scans in scan-to-scan, and the second is for caching the covariances of keyframe scans internally for future scan-to-map alignments so we can reuse them when building subsequent submaps.
The function this->gicp_s2s.swapSourceAndTarget() gets called before updateKeyframes(), which is also after the first time covariances are calculated. As you can probably guess, this swaps the source and target data structures (cloud, kdtree, and covariances) -- but what's important is that, after this swap, we just care about this->gicp_s2s's target data structures now since, at this point, we've already performed scan-to-scan and don't need the previous scan anymore. So we can just reuse the source structures as temporary storage as indicated by this comment.
One might wonder why we're computing covariances twice. Since scan-to-scan and scan-to-map operate in different coordinate systems (base_link vs odom), we need two sets of covariances (one for each alignment). One could certainly just transform the first set of covariances using GICP's output, but we found that calculating a single point cloud's covariances took negligible time (versus a variable-sized submap point cloud) and was just easier to implement this way when fixing this bug after initial release (thanks to @RyGuy101).
Let me know if that answers your question.
qweesiic
great job!
i found covariances was calculated in function updatekeyframe(); and i am not sure if
was executed after function updatekeyframe()， the time of covariances calculation in function updatekeyframe() maybe saved？