[RBF] Multi quaternion imput cause crash

[RDelet]

I noticed a crash when you have a setup with more than one quaternion inputs.
I share my test scene -> test_rbf.zip

In the code the crash comes from LinearRegressionSolver::solve when we exit the function. This is due to a memory overflow of inputDistance with the three nested for loops.
In my example, I have two quaternion inputs. theta_.cols() has a size of 8 but since we iterate twice over inputQuatCount and also over sampleCount we should have a size of 16.

Theta_ is set in LinearRegressionSolver::setFeatures with size of: valueCols + sampleCount * 2 * inputQuatCount.
I can avoid this crash with valueCols + sampleCount * 2 * (inputQuatCount * inputQuatCount) but in this case the second input do not have any modification on output value.

Since the code allows for multiple quaternion inputs to be connected, I'm thinking it's possible to have this kind of scenario, but given the result, I'm wondering if it's really what we want.
I'll try to dig a little deeper into the subject, but if you have any ideas on how to solve it simply, or just to see if it's not possible, that would be a great help in advance.

Thank you in advance for your response.

[RDelet]

I was able to work around the crash and make it work with multiple quaternion inputs with this code.
However, I'm not sure how the rotation decomposition relates to a pose. I'm not sure if it's really consistent to do this.

void LinearRegressionSolver::setFeatures(
    const MatrixXd& featureMatrix, const std::vector<std::vector<MQuaternion>>& featureQuatMatrix,
    const MatrixXd& outputScalarMatrix, const std::vector<MatrixXd>& outputQuats, short rbf,
    double radius, double regularization, SolverSpace space) {
    featureMatrix_ = featureMatrix;
    featureQuatMatrix_ = featureQuatMatrix;
    outputScalarMatrix_ = outputScalarMatrix;
    outputQuats_ = outputQuats;
    rbf_ = rbf;
    radius_ = radius;
    solverSpace_ = space;

    Eigen::Index sampleCount = featureMatrix_.rows() ? featureMatrix_.rows() : featureQuatMatrix_.size();
    if (sampleCount <= 1) {
        theta_.resize(0, 0);
        return;
    }
    Eigen::Index inputCount = featureMatrix_.cols();
    Eigen::Index inputQuatCount = featureQuatMatrix.size() ? featureQuatMatrix_[0].size() : 0;
    Eigen::Index valueCols = inputCount ? sampleCount : 0;
    // We will append the swing and twist distances for each input rotation
    // to the distance matrix
    Eigen::Index cols = valueCols + inputQuatCount * sampleCount * inputQuatCount * 2;

    MatrixXd m = MatrixXd::Zero(sampleCount, cols);

    if (inputCount) {
        featureNorms_.resize(inputCount);
        // Normalize each column so each feature is in the same scale
        for (int i = 0; i < inputCount; ++i) {
            featureNorms_[i] = featureMatrix_.col(i).norm();
            if (featureNorms_[i] != 0.0) {
                featureMatrix_.col(i) /= featureNorms_[i];
            }
        }

        for (Eigen::Index i = 0; i < sampleCount; ++i) {
            m.col(i) = (featureMatrix_.rowwise() - featureMatrix_.row(i)).matrix().rowwise().norm();
        }

        // Normalize distances
        distanceNorm_ = m.norm();
        m /= distanceNorm_;
    }

    applyRbf(m, rbf_, radius_);

    if (inputQuatCount) {
        sampleRadius_ = VectorXd::Ones(sampleCount);
        Eigen::Index colIdx = valueCols;
        double swingDistance, twistDistance;

        // Calculate rotation distances
        for (int inputQuatIdx = 0; inputQuatIdx < inputQuatCount; ++inputQuatIdx) {
            for (int s1 = 0; s1 < sampleCount; ++s1) {
                Eigen::Index startIdx = colIdx;
                for (int sampleQuatIdx = 0; sampleQuatIdx < inputQuatCount; ++sampleQuatIdx) {
                    for (int s2 = 0; s2 < sampleCount; ++s2) {
                        MQuaternion& q1 = featureQuatMatrix_[s2][inputQuatIdx];
                        MQuaternion& q2 = featureQuatMatrix_[s1][sampleQuatIdx];
                        swingTwistDistance(q1, q2, swingDistance, twistDistance);
                        if (solverSpace_ == SolverSpace::Swing) {
                            twistDistance = 0.0;
                        }
                        else if (solverSpace_ == SolverSpace::Twist) {
                            swingDistance = 0.0;
                        }
                        // TODO: Each feature quat should have it's own radius
                        if (swingDistance > 0.000001 && swingDistance < sampleRadius_[s2]) {
                            sampleRadius_[s2] = swingDistance;
                        }
                        if (twistDistance > 0.000001 && twistDistance < sampleRadius_[s2]) {
                            sampleRadius_[s2] = twistDistance;
                        }

                        m(s2, colIdx) = swingDistance;
                        m(s2, colIdx + 1) = twistDistance;
                    }
                    colIdx += 2;
                }
                auto m_segment = m.block(0, startIdx, sampleCount, inputQuatCount * 2);
                applyRbf(m_segment, rbf_, sampleRadius_[s1] * radius_);
            }
        }
    }

    // Rather than solve directly to the output values, we will store 0 or 1 pose values.
    // This lets us calculate the output as a linear combination of the sample outputs and
    // will make it easier to calculate output quaternions
    MatrixXd outputMatrix = MatrixXd::Identity(sampleCount, sampleCount);
    MatrixXd r = MatrixXd::Zero(cols, cols);
    r.diagonal().array() = regularization;

    MatrixXd tm = m.transpose();
    MatrixXd mat = pseudoInverse(tm * m + r) * tm;
    theta_ = (mat * outputMatrix).transpose();
}