Hesai QT128 with Zed2i and external fused Odometry

[aninath93]

This is causing the issue from the Grip map integration, updated pose is not found in node after tag detection and map get cleared,

https://github.com/introlab/rtabmap/blob/master/corelib/src/GlobalMap.cpp#L131

I thought the tags were not added as nodes, just kept as links which get destroyed once seen and optimized. If i comment this out, it works and the map does not revert back, does a new condition needs to be added for this
if graphChanged && tag_detections ?

        if(graphOptimized || graphChanged)
        {
                // clear all but keep cache
                //clear();
        }

[aninath93]

Hi Mathieu,

I have a solution, if you accept I can make PR but the theory is to just delete the links which were added as nodes by seeing tags and deleting those which do not contain any Pose value only instead of deleting all the added nodes with poses and recreating which works after testing as the nodes were deleted.

bool GlobalMap::update(const std::map<int, Transform> & poses)
{
        UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());

        // First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
        bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
        bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
        float updateErrorSqrd = updateError_*updateError_;
       
        for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
        {
                std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
                if(jter != poses.end())
                {
                        graphChanged = false;
                        UASSERT(!iter->second.isNull() && !jter->second.isNull());
                        if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
                        {
                                graphOptimized = true;
                        }
                }
                else
                {
                        extraNodesToClear_.insert(iter->first);  //added this to the .h file which is extranodes which we get while iteration above which do not have the pose.



                        UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", iter->first);
                }
        }

        if(graphOptimized || graphChanged)
        {
                // clear all but keep cache
                clear();
        }

        std::list<std::pair<int, Transform> > orderedPoses;

        // add old poses that were not in the current map (they were just retrieved from LTM)
        for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
        {
                if(!isNodeAssembled(iter->first))
                {
                        UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
                        orderedPoses.push_back(*iter);
                }
        }

        // insert zero after
        if(poses.find(0) != poses.end())
        {
                orderedPoses.push_back(std::make_pair(-1, poses.at(0)));
        }

        if(!orderedPoses.empty())
        {
                assemble(orderedPoses);
        }

        return !orderedPoses.empty();
}


then clear fucntion as

void GlobalMap::clear()
{
        UDEBUG("Clearing");
        //addedNodes_.clear();
        extraNodesToClear_.clear();
        minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
        maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
}

[matlabbe]

Closing this thread, continuing discussion on https://github.com/introlab/rtabmap/issues/1465