Pointcloud modifications and republishing

Dear Nvidia,

I’ve just started using your software and I’ve ran into an issue.

What I would like to do is:

  1. Subscribe to a PointCloudProto topic
  2. Do modifications/calculations on raw PointCloud data
  3. Republish the modified PointCloud

As a primer I tried to simply create an “Echo” node that subscribes and publishes the same pointcloud:

reference.cpp:

#include “reference.hpp”

void Reference::start() {
tickOnMessage(rx_cloud_in());
}

void Reference::tick() {
PointCloudProto::Reader proto_in = rx_cloud_in().getProto();
PointCloudProto::Builder proto_out = tx_cloud_out().initProto();

if (proto_in.hasPositions()) {
    proto_out.setPositions(proto_in.getPositions());
    LOG_INFO("Received position");
}
if (proto_in.hasNormals()) {
    proto_out.setNormals(proto_in.getNormals());
    LOG_INFO("Received normals");
}
if (proto_in.hasIntensities()) {
    proto_out.setIntensities(proto_in.getIntensities());
    LOG_INFO("Received intensities");
}
if (proto_in.hasColors()) {
    proto_out.setColors(proto_in.getColors());
    LOG_INFO("Received colors");
}
tx_cloud_out().publish();

}

void Reference::stop() {}

reference.hpp

#pragma once
#include “engine/alice/alice_codelet.hpp”
#include “messages/point_cloud.capnp.h”

class Reference : public isaac::alice::Codelet {
public:
void start() override;
void tick() override;
void stop() override;
ISAAC_PROTO_RX(PointCloudProto, cloud_in);
ISAAC_PROTO_TX(PointCloudProto, cloud_out);
};
ISAAC_ALICE_REGISTER_CODELET(Reference);

These are the edges of the system:

    "edges": [
        {
            "source": "vlp16/isaac.velodyne_lidar.VelodyneLidar/scan",
            "target": "scan_accumulator/isaac.perception.ScanAccumulator/scan"
        },
        {
            "source": "scan_accumulator/isaac.perception.ScanAccumulator/fullscan",
            "target": "point_cloud/isaac.perception.RangeToPointCloud/scan"
        },
        {
            "source": "point_cloud/isaac.perception.RangeToPointCloud/cloud",
            "target": "reference/reference/cloud_in"
        },
        {
            "source": "reference/reference/cloud_out",
            "target": "hgmm/isaac.hgmm.HgmmPointCloudMatching/cloud"
        }   
    ]

Now the problem that I ran into, this is what gets printed to console:

2020-08-10 15:49:46.263 INFO packages/bridge/reference.cpp@13: Received position
2020-08-10 15:49:46.263 INFO packages/bridge/reference.cpp@21: Received intensities
2020-08-10 15:49:46.263 ERROR messages/tensor.cpp@49: Buffer index 0 out of range (0):

So the node DOES get triggered, the new proto_out function does write data, yet the receiver (hgmm) does not function. It does not compute a pose. Where is this tensor located?

The system DOES work if i bypass my own node, like this:

    "edges": [
        {
            "source": "vlp16/isaac.velodyne_lidar.VelodyneLidar/scan",
            "target": "scan_accumulator/isaac.perception.ScanAccumulator/scan"
        },
        {
            "source": "scan_accumulator/isaac.perception.ScanAccumulator/fullscan",
            "target": "point_cloud/isaac.perception.RangeToPointCloud/scan"
        },
        {
            "source": "point_cloud/isaac.perception.RangeToPointCloud/cloud",
            "target": "reference/reference/cloud_in"
        },
        {
            "source": "point_cloud/isaac.perception.RangeToPointCloud/cloud",
            "target": "hgmm/isaac.hgmm.HgmmPointCloudMatching/cloud"
        }   
    ]

Assuming that this is an easy fix, I would also like to know how I can easily inspect the raw pointcloud data to a form that i can easily/intuitively perform math on.

So i tried going the route of FromProto/ToProto of the pointcloud Tensors:

void Reference::tick() {
auto proto_in = rx_cloud_in().getProto();
auto positions = proto_in.getPositions();
auto intensities = proto_in.getIntensities();

auto proto_out = tx_cloud_out().initProto();
auto positions_out = proto_out.initPositions();
auto intensities_out = proto_out.initIntensities();

isaac::TensorBase<float_t, isaac::tensor::Dimensions<isaac::tensor::kDynamic, 1>, isaac::CpuBufferConstView> tensor_position{};
isaac::TensorBase<float_t, isaac::tensor::Dimensions<isaac::tensor::kDynamic, 1>, isaac::CpuBufferConstView> tensor_intensities{};

FromProto(positions, rx_cloud_in().buffers(), tensor_position);
FromProto(intensities, rx_cloud_in().buffers(), tensor_intensities);

ToProto(tensor_position, positions_out, tx_cloud_out().buffers());
ToProto(tensor_intensities, intensities_out, tx_cloud_out().buffers());

tx_cloud_out().publish();

}

But this gives me an error in:

In file included from packages/bridge/reference.cpp:2:0:
./messages/tensor.hpp: In instantiation of ‘void ToProto(isaac::TensorBase<K, Dimensions, BufferType>, TensorProto::Builder, std::vectorisaac::SharedBuffer&) [with K = float; Dimensions = std::integer_sequence<int, -1, 1>; BufferType = isaac::detail::BufferBase<isaac::detail::TaggedPointer<const unsigned char, std::integral_constant<isaac::BufferStorageMode, (isaac::BufferStorageMode)0> > >]’:
packages/bridge/reference.cpp:33:69: required from here
./messages/tensor.hpp:68:3: error: static assertion failed: Must own the data to send it.
static_assert(isaac::BufferTraits::kIsOwning, “Must own the data to send it.”);
^~~~~~~~~~~~~
src/main/tools/linux-sandbox-pid1.cc:437: waitpid returned 2
src/main/tools/linux-sandbox-pid1.cc:457: child exited with code 1
src/main/tools/linux-sandbox.cc:204: child exited normally with exitcode 1
Target //packages/bridge:reference failed to build
Use --verbose_failures to see the command lines of failed build steps.
INFO: Elapsed time: 2.773s, Critical Path: 2.66s
INFO: 0 processes.
FAILED: Build did NOT complete successfully
The terminal process “/bin/bash ‘-c’, ‘bazel build -c dbg --strip=never --copt=-O0 //packages/bridge:reference --copt=-Wno-error --sandbox_debug’” terminated with exit code: 1.

So how do i “own” the data? I assume I have to copy it locally in the node instead of a const view? How is this done?

and how does this relate to the question of:

2020-08-10 15:49:46.263 ERROR messages/tensor.cpp@49: Buffer index 0 out of range (0):

Did this error mean that i’ve been sending empty PointCloudProtos to the HGMM node?

Greetings and thank you,

Deniz

Hi Deniz,

Let me try to explain what’s going on here:
Messages contain two different parts:

  • A Cap’n Proto message
  • A list of buffers
    The reason behind the buffers was to avoid constant memory allocation, so when a Codelet wants to publish a lot of data, instead of serializing everything, and then de-serialize everything in the comsumer, you can instead give up ownership of the memory and create a buffer, and the Cap’n Proto message will contains the pointer to the buffer. In the consumer you can simply create a view (ImageConstView, TensorConstView) and avoid a copy.
    It’s important to note that the message is now owning the buffers, and they have to be declared as const given multiple Codelet can receive and read in the same time the same message.

So the reason behind the first error:

2020-08-10 15:49:46.263 ERROR messages/tensor.cpp@49: Buffer index 0 out of range (0):

When you copied the tensors, you only copied the Cap’n Proto part, the message itself refers to a buffer index (from the original message), and you did not copy the buffers. It is not really you sent an empty PointCloudProtos but rather an incomplete one, the proto refers to a not existing buffer.

As you noted, it’s better in general to use the FromProto and ToProto functions.
Now you’re getting another error (at compilation time), because when you call FromProto, in order to avoid a copy of the data, it creates a TensorConstView, as the name indicates, it is only a view, it does not own the data (the data is still own by the message, and cannot be transfer as you have to remember other Codelet could be also reading the same message). It is Const for similar reason, it would be dangerous to modify it without protection.
So unfortunately you cannot use a View to create a message, you need to own the memory in order to transfer it to the message:

  SampleCloud3f positions;    // Just a tensor
  SampleCloud1f intensities;  // Just a tensor
  .....
  proto.initPositions();
  proto.initIntensities();
  ToProto(std::move(positions), proto.getPositions(), tx_cloud_out().buffers());
  ToProto(std::move(intensities), proto.getIntensities(),
          tx_accumulated_point_cloud().buffers());

So unfortunately, as you guessed, you’re going to need to make a copy of the Tensor and then modify it.
We don’t have at the moment a way to give up memory from messages as it can create some issues when a same message have several consumers.
To make a copy, you can create a tensor of the same size and then just copy the data:

#include “engine/gems/tensor/utils.hpp”

Tensor2d output(tensor_position.dimensions());
Copy(tensor_position, output)

I hope it helps, let me know if you run into more issues.

Regards,

Ben

1 Like