COLOR to DEPTH mapping issues/confusion

[Suedocode]

I am attempting to create a colored depth frame in the depth camera geometry, so mapping COLOR to DEPTH. I've attempted this so far modeled after the coordinate_transform example code, but the results were very off-center. I'm sure I'm just misusing the various transformation parameters and could use some guidance on which parameters go where. I've only cut out the relevant pieces:

static constexpr uint32_t s_fps = 15;
static constexpr uint32_t s_dw = 640;
static constexpr uint32_t s_dh = 576;
static constexpr uint32_t s_dsz = s_dw*s_dh*2;
static constexpr uint32_t s_cw = 1920;
static constexpr uint32_t s_ch = 1080;
static constexpr uint32_t s_csz = s_dw*s_dh*4;

int main() {
  ob::Context ctx;
  auto mDev = ctx.queryDeviceList()->getDevice(0);
  auto mPipeline = std::make_shared<ob::Pipeline>(mDev);

  auto colorProfile =  mPipeline->getStreamProfileList(OB_SENSOR_COLOR)->getVideoStreamProfile(s_cw, s_ch, OB_FORMAT_BGRA, s_fps);
  auto depthProfile =  mPipeline->getStreamProfileList(OB_SENSOR_DEPTH)->getVideoStreamProfile(s_dw, s_dh, OB_FORMAT_Y16,  s_fps);

  OBCameraIntrinsic d_intr = depthProfile->as<ob::VideoStreamProfile>()->getIntrinsic();
  OBCameraDistortion d_dist = depthProfile->as<ob::VideoStreamProfile>()->getDistortion();
  OBExtrinsic ext_d2c = depthProfile->getExtrinsicTo(colorProfile);
  OBExtrinsic ext_d2d = depthProfile->getExtrinsicTo(depthProfile);

  OBCameraIntrinsic c_intr = colorProfile->as<ob::VideoStreamProfile>()->getIntrinsic();
  OBCameraDistortion c_dist = colorProfile->as<ob::VideoStreamProfile>()->getDistortion();
  OBExtrinsic ext_c2d = colorProfile->getExtrinsicTo(depthProfile);

  static const uint8_t bgra_default[4] = {255};
  auto get_color_pix = [&](int w, int h, const uint8_t* data) -> const uint8_t* {
    if (w < 0 || w >= s_cw ||
        h < 0 || h >= s_ch) return bgra_default;
    return data + (h*s_cw + w)*4;
  };

//looping for each frame
    auto df = frameset->depthFrame();
    auto cf = frameset->colorFrame();
    const uint16_t* depth_data = (const uint16_t*)(df->getData());
    const uint8_t* color_data = (const uint8_t*)(cf->getData());

    size_t i = 0;
    for (int h=0; h<s_dh; ++h) {
      for (int w=0; w<s_dw; ++w) {
        PointXYZRGB& p = cld.points[i];
        float d = float(depth_data[i++]);
        if (d == 0) {
          memset(&p, 0, sizeof(p));
          continue;
        }

        OBPoint2f src_pix {static_cast<float>(w), static_cast<float>(h)};
        OBPoint3f tgt_p {};

        ob::CoordinateTransformHelper::transformation2dto3d(
            src_pix, d, d_intr, ext_d2d, &tgt_p);

        p.x = tgt_p.x*.001;
        p.y = tgt_p.y*.001;
        p.z = tgt_p.z*.001;

        OBPoint2f c_pix {};

        ob::CoordinateTransformHelper::transformation2dto2d(
            src_pix, d,
            d_intr, d_dist,
            c_intr, c_dist,
            ext_d2c, &c_pix);

        const uint8_t* bgra = get_color_pix(c_pix.y, c_pix.x, color_data);
        p.b = bgra[0];
        p.g = bgra[1];
        p.r = bgra[2];
      }
    }

I can post the full runnable code if necessary.

I also tried the ob::Align stuff which looks promising, but the api doesn't seem to incorporate intrinsics yet and it yields this message:

[error][52963][AlignImpl.cpp:652] Not supported yet 

Environment: Ubuntu 22.04.5 LTS (x86_64) g++ 11.4.0 OrbbecSDK-dev bd2d4dd41d6214c9d8bfe3365efa865d5fe8df9f Firmware v1.2.9

[hzcyf]

Thank you for your feedback. Due to the National Day holiday in China, most of our team is on vacation, which has delayed our response. We will address your issues and needs as soon as we return to work tomorrow.

[hzcyf]

The problem you’re encountering is likely due to a bug in your code. You should modify the line:

const uint8_t* bgra = get_color_pix(c_pix.y, c_pix.x, color_data);

to:

const uint8_t* bgra = get_color_pix(c_pix.x, c_pix.y, color_data);

[hzcyf]

With ob::Align, we don’t yet support C2D processing for BGRA formatted images. You might want to try using RGB format instead. If that’s not convenient, you can also wait for our next update, which should include support for BGRA and is expected to be released in about a week.

[Suedocode]

Oh my gosh inverting the pixel coordinates fixed it. My bad!

I'm interested in the simplicity of ob::Align, however I need to be able to load extrinsics from disk and apply them to images from that device. I didn't quite understand the code that I saw, but it seemed like it was trying to load extrinsics from meta data within the image itself, which will not be available when I'm simulating situations without a camera connected.

That said, these new CoordinateTransformHelpers might be good enough because they do seem quite a bit faster than the legacy version.

[hzcyf]

The extrinsics between frames is actually bound to their StreamProfile. Frame provides an interface to get its StreamProfile, and StreamProfile provides an API to get extrinsics.

StreamProfile currently also provides an interface register extrinsics from the user side, which can meet your needs of loading and applying extrinsics from disk.

However, this is indeed very complicated to use. We will consider providing an interface in ob::Align to directly input user-defined extrinsics.