Arm reconstruction from 360 scan via rgbd odometry and ScalableTSDFVolume

[DuccioLenkowicz]

I'm attempting to do an arm reconstruction using a 360 scan captured from a realsense d435 camera. This is the code i'm using:

class ReconstructionSystem{
public:
    explicit ReconstructionSystem(const rs2_intrinsics &colorIntrinsic, const rs2_intrinsics &depthIntrinsic, double depthScale, double maxDepth) : 
        m_source(0), 
        m_colorIntrinsic(rs2Inrinsics2PinholeCameraIntrinsic(colorIntrinsic)), 
        m_depthIntrinsic(rs2Inrinsics2PinholeCameraIntrinsic(depthIntrinsic)),
        m_depthScale(depthScale), 
        m_cameraTransform(Eigen::Matrix4d::Identity()), 
        m_volume(0.1 / 100, 0.0085, TSDFVolumeColorType::Gray32),
        m_maxDepth(maxDepth),
        m_option({20, 10, 5}, 0.03, 0.01, maxDepth)
    {
    }
    void init(const rs2::frameset &alignSource, const rs2::frameset &integrationSource, const Eigen::Matrix4d &cameraTransform=Eigen::Matrix4d::Identity()){
        m_cameraTransform = cameraTransform;
        m_source = frame2RGBD(alignSource, m_depthScale, m_maxDepth);
        m_volume.Reset();
        m_volume.Integrate(
            *frame2RGBD(integrationSource, m_depthScale, m_maxDepth), 
             m_depthIntrinsic, m_cameraTransform);
    }
    bool update(const rs2::frameset &alignTarget, const rs2::frameset &integrationTarget) 
    {
        std::shared_ptr<RGBDImage> target = frame2RGBD(alignTarget, m_depthScale, m_maxDepth);
        auto result = ComputeRGBDOdometry(
            *m_source, *target, 
            m_colorIntrinsic, Eigen::Matrix4d::Identity(), 
            RGBDOdometryJacobianFromHybridTerm(), m_option
        );

        if(!std::get<0>(result)) { return false; }

        auto transform = std::get<1>(result);
        m_cameraTransform = transform * m_cameraTransform;
        m_volume.Integrate(
            *frame2RGBD(integrationTarget, m_depthScale, m_maxDepth), 
            m_depthIntrinsic, m_cameraTransform);
        m_source = target;
        return true;
    }
    std::shared_ptr<open3d::PointCloud> computePointCloud(){
        return m_volume.ExtractPointCloud();
    }

private:
    std::shared_ptr<RGBDImage> m_source;
    PinholeCameraIntrinsic m_colorIntrinsic;
    PinholeCameraIntrinsic m_depthIntrinsic;
    ScalableTSDFVolume m_volume;
    double m_depthScale;
    double m_maxDepth;
    Eigen::Matrix4d m_cameraTransform;
    OdometryOption m_option;
};

Here you see a couple of pictures of the resulting point cloud after voxel downsampling and euclidean clustering.

As you can see the arm surface does not properly close upon itself, like if the camera trajectory is being distorted into a spiral instead of being a circle as it should. Any idea on what maybe the cause of this issue?

[syncle]

Hi @DuccioLenkowicz Thanks for sharing a new C++ example of Reconstruction system! The code looks great. The only reason I can think is that it does not have explicit loop closure detection steps. For example, in Python example of Reconstruction system it builds submaps (or called fragments) and register them globally to perform loop closure detection.

Here are two suggestion

• Can you feed the captured frame to Python Reconstruction System? In this way, you can compare the effect on loop-closure.
• Can you also capture static objects? It would be good to know if it is because of the target scene is not static (arm). That might be good to decouple possible reasons.