Support for Kinematic ICP

[eshafeeqe]

Dear Opt,

I tried to implement Kinematic ICP with Opt, as you may aware that Kinematic graph have uneven graph lengths for different paths. So I implemented an even graph by putting dummy variables and made weights zero for those dummy variables. as shown below.

local N, B = Dim("N",0), Dim("B",2)
local Gfit = Graph("Gfit", 1,
                 "V",  {N}, 2,
                 "b0", {B}, 3,
                 "b1", {B}, 4,
                 "b2", {B}, 5,
                 "b3", {B}, 6,
                 "b4", {B}, 7)
-- Note that for each point in the cloud the corresponding 5 levels of bones are passed in the edges b0 to b5

local SkeletonIndexWeights = Array("SkeletonIndexWeights", opt_float5,{N}, 8) -- kept weight to 1 for actual bone and weight to zero for dummy bone for making the graph uniform 

local InitPoints = Array("InitPoints", opt_float3,{N},9) -- initial points
local Constraints = Array("Constraints", opt_float3,{N},10) -- target point
local Normals = Array("Normals", opt_float3,{N},11) --  target normal

-- Defined some supporting functions
function Matrix3xMatrix3Mul(matrix1,matrix2)
        return ad.Vector(

            matrix1(0)*matrix2(0)+matrix1(1)*matrix2(3)+matrix1(2)*matrix2(6),
            matrix1(0)*matrix2(1)+matrix1(1)*matrix2(4)+matrix1(2)*matrix2(7),
            matrix1(0)*matrix2(2)+matrix1(1)*matrix2(5)+matrix1(2)*matrix2(8),

            matrix1(3)*matrix2(0)+matrix1(4)*matrix2(3)+matrix1(5)*matrix2(6),
            matrix1(3)*matrix2(1)+matrix1(4)*matrix2(4)+matrix1(5)*matrix2(7),
            matrix1(3)*matrix2(2)+matrix1(4)*matrix2(5)+matrix1(5)*matrix2(8),

            matrix1(6)*matrix2(0)+matrix1(7)*matrix2(3)+matrix1(8)*matrix2(6),
            matrix1(6)*matrix2(1)+matrix1(7)*matrix2(4)+matrix1(8)*matrix2(7),
            matrix1(6)*matrix2(2)+matrix1(7)*matrix2(5)+matrix1(8)*matrix2(8))
     end

 function Euler2Rot(a)
        local alpha, beta, gamma = a(0), a(1), a(2)
        local  CosAlpha, CosBeta, CosGamma, SinAlpha, SinBeta, SinGamma = ad.cos(alpha), ad.cos(beta), ad.cos(gamma), ad.sin(alpha), ad.sin(beta), ad.sin(gamma)
        return ad.Vector(
            CosGamma*CosBeta,
            -SinGamma*CosAlpha + CosGamma*SinBeta*SinAlpha,
            SinGamma*SinAlpha + CosGamma*SinBeta*CosAlpha,
            SinGamma*CosBeta,
            CosGamma*CosAlpha + SinGamma*SinBeta*SinAlpha,
            -CosGamma*SinAlpha + SinGamma*SinBeta*CosAlpha,
            -SinBeta,
            CosBeta*SinAlpha,
            CosBeta*CosAlpha)
    end

local function rotationSkeleton(w,x)
    return Euler2Rot(w*RotationBones(x))
end

-- SInce the blobal transformation comes as the product from root to child it will come 5 recusrive multiplications
local skeleton_tranformed = Matrix3x3Mul(Matrix3xMatrix3Mul(Matrix3xMatrix3Mul(Matrix3xMatrix3Mul(Matrix3xMatrix3Mul(rotationSkeleton(SkeletonIndexWeights(Gfit.w)(0),Gfit.b0), rotationSkeleton(SkeletonIndexWeights(Gfit.w)(1),Gfit.b1)),
            rotationSkeleton(SkeletonIndexWeights(Gfit.w)(2),Gfit.b2)),rotationSkeleton(SkeletonIndexWeights(Gfit.w)(3),Gfit.b3)),
            rotationSkeleton(SkeletonIndexWeights(Gfit.w)(4),Gfit.b4)), InitPoints(Gfit.w))


local e_skeleton = Dot(skeleton_tranformed -Constraints(Gfit.w),Normals(Gfit.w))

local valid = greatereq(Constraints(Gfit.w,0,0), -999999.9)
Exclude(lesseq(Constraints(Gfit.w,0,0), -999999.9))
Energy(Select(valid, w_skelSqrt*e_skeleton, 0))

The good thing is it worked fine, but I have two problems now

1. It took about 16 seconds for problem plan completion since I have a variable point cloud at each iteration I need to resize dims at each iteration.
2. I have to pass skeleton chain for each point in the point cloud externally since the skeleton chain for each bone is fixed and is there any alternative for this?

[Mx7f]

1. This is the biggest outstanding issue with Opt, the simple workaround is to do the trick you did for the chain size, and just compile the largest size and use dummy vars, but hopefully we will support dynamic dimensions in the near future. Also, I will take a look at the compilation time itself, since 16s is longer than it seems like it needs to be.

2. Couldn't you pass the bone index for each point and store the skeleton chain per bone?