Issue on Object Tracking

[HUmutOzdemir]

Hi, I am trying to use this mapping framework with iTHOR simulator. Currently mapping framework is used with ground truth pose and ground truth pointclouds which are obtained via 3D reprojection of image pixels with ground truth depth information. I run into this issue during my experiments. TSDF++ doesn't fusing previous observations if an object is tracked.

Experiment Setup

1. Robot initially starts with this view and after 0.2 meters left movement, the mesh is generated using /tsdf_plusplus_ros/generate_mesh ROS servise.

2. Then, robot crouches and after 0.2 meters left movement, another mesh is generated. The view of ther crouch action is as follows:

Please note that during this experiment, all objects are stationary. Therefore, ICP part in object tracking is commented out. (Line 408-409 in controller.cc.

Generated Meshes

Whole scene

The following screenshots are the meshes generated after each step of the experiment.

Mesh 1

Mesh 2

As it can be seen, in the second mesh, some parts of the objects is not fused with previous observations, e.g. top part of the first sofa.

Only sofa

In this part, I only published the point cloud of the initial sofa in the environment.

Mesh 1

Mesh 2

Only sofa without object tracking

In this part, I only published the point cloud of the initial sofa in the environment. Additionally, I disabled the object tracking by commenting out the line 425 in controller.cc (map_->transformLayer(segment->object_id_, T_O_S);).

Mesh 1

Mesh 2

In this part, second mesh also contains the previous observations. Could you please help me? Thanks in advance.

[margaritaG]

@HUmutOzdemir

There are two things that come to mind:

1. As you might have noticed, there are some hardcoded checks on the size of the per-frame segments in https://github.com/ethz-asl/tsdf-plusplus/blob/main/tsdf_plusplus_ros/src/controller.cc#L344. Please double check that the sofa segment size doesn't cause issues or constant switching between tracking ON/OFF for this object.
2. It could be that ICP simply failed to track the sofa segment against the sofa object in the map, and hence when you actually call transformLayer it "moves" the sofa object somewhere random, hence effectively deleting what was previously reconstructed, and then integrates the new frame (which will of course lack whatever was observed and fused before).

I'm not sure if you are actually interested in tracking the sofa, if not, then you could just fine tune the segment size thresholds mentioned in 1 (quick and dirty) or exclude the sofa class from tracking (more involved but nicer solution).

Something else which might be helpful is looking at the real-time visualizer of the reconstructed scene, rather than just generating a mesh once in a while (you have higher chances of catching your sofa flying away if it's being incorrectly tracked :) ). Lastly, you could inspect the actual poses produced by ICP and try to see if they are making any sense or it the sofa tracking indeed is failing miserably.

[HUmutOzdemir]

@margaritaG

1. As you suggest, I am using the ground truth object classes to decide if an object will be tracked or not. Therefore, I am sure that there is not an ON/OFF problem in this case. Actually, the problem is not only the sofa. It also can not track other small objects, too. I gave this example because it is easier to give the example of the sofa.
2. For the ICP part, I disabled the ICP part. Because all the objects are static, the ICP would give identity transformation. That's why, to exclude the effect of ICP errors, I removed the ICP part in all of the experiments and gave identity transformation for each tracked object.

[margaritaG]

@HUmutOzdemir

I am confused by the specifics of what you are currently running. You mention that you disabled the ICP part, but then why are you even injecting any transformations for each object? If you want to correctly disable object tracking - you can just set object_tracking_enabled_ to false (https://github.com/ethz-asl/tsdf-plusplus/blob/main/tsdf_plusplus_ros/src/controller.cc#L274). Have you tried this? Does the sofa get fully reconstructed over time? I would expect so.

If I understand correctly, what you are doing instead is still keeping object tracking on, but then manually injecting some identity poses rather than letting ICP compute their pose. Well if this causes the failures you are seeing, while the previously mentioned proper solution does not, then I have two guesses:

1. You might want to double check that the poses you are injecting are correct, I encourage again the use of the interactive visualizer paired with stepping through the rosbag manually, so as to observe what happens right after transformLayer is called, before the next frame is processed and fused.
2. When transforming an object reconstruction, under the hood there's going to be a trilinear interpolation of TSDFs in voxels. Voxels that do not have all the necessary neighbours for a complete trilinear interpolation of their TSDF won't be "moved" to the destination location. So perhaps you are fusing so few frames that your surfaces are very "thin" and very few voxels have the neighbours necessary to get that bit of the surface to be moved to the new location? In that case, they would first be deleted, and then they simply wouldn't be written to the destination location (even if it's an identity transformation). That's just how `transformLayer' works.

You could also try looking at the individual objects model through the save_objects service. It will save as meshes the reconstructed individual objects, independently of whether they are active or inactive in the global map. Calling this service in between the different frames coming in (again, manually stepping through the rosbag) can help you double check that the issue is reconstruction itself, and not just inactive vs active voxels.

[HUmutOzdemir]

@margaritaG

Thank you for the answer. Actually, I want to use object tracking functionality. However, I realized that current setup doesn't fuse the previous observations. That's why I created this setup to find the reason of this bug. What I mean by disabling ICP is that commenting out line 408. I also tried to set object_tracking_enabled_ to false. In this case sofa is reconstructed. But I actually need object tracking, and sofa is one of the objects that is able to move.

1. I checked the poses I gave as input using rviz. I couldn't find a mistake. I also tried to use interactive visualizer, but it always crashed when the robot is crouched. That's why I can not use visualizer.

2. This could be the reason. Because as you said, for the initial reconstruction, 3 frames are fused. I commented out line 308 in map.cc to check that was the reason. But doing that didn't fix the problem either. I obtaned same reconstructions as before. What is your opinion about this situation? Can I check differently if this is the cause?

I also tried to use save_objects service. When I called the service, it generates the .ply files of each object. But when it's time for the last object, the program crashes.

Do you have any idea on why visualizer and save_objects service crashes?

[margaritaG]

@HUmutOzdemir

1. I'm not sure what is it about the robot crouching that makes the visualizer crash - I'm not saying that I've never seen the visualizer crash, but I just fail to see what could it be about the robot crouching that makes it crash, it's just a pose like any other. Without more specific on the crash/error output, the only thing that comes to mind is that there is a race condition or something of the sort, where the visualizer is trying to compute the scene mesh while the underling TSDF is still being updated.
2. One last way in which you could debug how the sofa layer is transformed is expose a dummy service which would just call transformLayer on the sofa object, for a hardcoded pose. I would fuse a bunch of frames, then pause the rosbag (such that no new frames are being fused), and then when you're sure that the last frame has been properly fused, call the new service to transform the sofa object. I would give it some pose that is not identity, but some pose which would lift it somewhere in the sky. It would be helpful if you would be running the visualization of the entire map either through the rviz subscription to the scene mesh or through the interactive visualizer. You should then see how the transformed sofa looks - if suddenly there are missing parts caused just by the transformLayer call, then it means that your sofa reconstruction was too limited and the transformation through tri-linear interpolation "eats away" some of the reconstructed voxels/surface.

When calling save_objects, you should be careful about when calling it - you should make sure that there's no fusion in progress, and no scene mesh update in progress, else there will be a race condition where you're trying to write to file data that is still being updated. It was a service that I have just implemented for debugging/visualization purposes, hence why it's not guarded against race conditions.

[HelloTaeyeon]

@margaritaG

Thank you for the answer. Actually, I want to use object tracking functionality. However, I realized that current setup doesn't fuse the previous observations. That's why I created this setup to find the reason of this bug. What I mean by disabling ICP is that commenting out line 408. I also tried to set object_tracking_enabled_ to false. In this case sofa is reconstructed. But I actually need object tracking, and sofa is one of the objects that is able to move.

1. I checked the poses I gave as input using rviz. I couldn't find a mistake. I also tried to use interactive visualizer, but it always crashed when the robot is crouched. That's why I can not use visualizer.
2. This could be the reason. Because as you said, for the initial reconstruction, 3 frames are fused. I commented out line 308 in map.cc to check that was the reason. But doing that didn't fix the problem either. I obtaned same reconstructions as before. What is your opinion about this situation? Can I check differently if this is the cause?

I also tried to use save_objects service. When I called the service, it generates the .ply files of each object. But when it's time for the last object, the program crashes.

Do you have any idea on why visualizer and save_objects service crashes?

After the installation is complete, how should I run this project? Do I need to add some code?