Bundle Adjustment

[RamadanAhmed]

Hi Thanks for open sourcing this project, I was trying to use it in solving bundle adjustment problem but it always gives me error "Graph element is in a different index space from image". the cost function is the one used by ceres-solver bundle Adjustment Example if some one can help me to trace the error, It will help a lot

Code

local cameraCount       = Dim("cameraCount", 0)
local point3DCount      = Dim("point3DCount", 1)
local observationsCount = Dim("Observations", 2)

local cameraType       = double9
local pointType        = double3
local observationsType = double2

local cameras      = Unknown("cameras", cameraType , {cameraCount},  0) -- translation.xyz, rotation.xyz , f,k1,k2
local points3d     = Unknown("points3d", pointType  ,{point3DCount}, 1) -- Pt x, y, z

local observations    = Array("observations",    observationsType, {observationsCount}, 2)  -- Pt x, y

local G = Graph("G", 3,
                "CamerasIndex",  {observationsCount} ,4,
                "points3dIndex", {observationsCount}, 5,
                "observations",  {observationsCount}, 6)

local function cross(ptA, ptB)
    return Vector( ptA(1) * ptB(2) - ptA(2) * ptB(1),
                   ptA(2) * ptB(0) - ptA(0) * ptB(2),
                   ptA(0) * ptB(1) - ptA(1) * ptB(0) )
end

local function rotatePoint(axisAngle, pt)
    local theta2 = axisAngle:dot(axisAngle)

    local theta = sqrt(theta2)
    local cosTheta = cos(theta)
    local sinTheta = sin(theta)
    local thetaInverse = 1.0 / theta

    local w = axisAngle * thetaInverse
    local crossWPt = cross(w, pt)
    local tmp = w:dot(pt) * (1.0 - cosTheta)

    local full = pt * cosTheta + crossWPt * sinTheta + w * tmp
    local simple = pt + cross(axisAngle, pt)

    return Select(greatereq(theta2, 1e-6),full,simple)
end

local function apply(camera, pt)
    local world = rotatePoint(Vector(camera(0), camera(1), camera(2)), pt)
    return world + Vector(camera(3), camera(4), camera(5))
end

UsePreconditioner(true)
--Reprojection cost Function
local observation = observations(G.observations)
local camera      = cameras(G.CamerasIndex)

local point3d     = points3d(G.points3dIndex)

local world = rotatePoint(Vector(camera(0), camera(1), camera(2)), point3d)
local p     = world + Vector(camera(3), camera(4), camera(5))

local xp = - p(0) / p(2)
local yp = - p(1) / p(2)

local l1 = camera(7)
local l2 = camera(8)
local r2 = xp * xp  + yp * yp
local distortion = 1.0 + r2 * (l1 + l2 * r2)
local focal = camera(6)

local predicted = focal * distortion * Vector(xp,yp)

Energy(predicted - observation)

[Mx7f]

local G = Graph("G", 3,
                "CamerasIndex",  {observationsCount} ,4,
                "points3dIndex", {observationsCount}, 5,
                "observations",  {observationsCount}, 6)

should be

local G = Graph("G", 3,
                "CamerasIndex",  {cameraCount} ,4,
                "points3dIndex", {point3DCount}, 5,
                "observations",  {observationsCount}, 6)

since the camera and points indices are used to index into images of size cameraCount and point3DCount, respectively.

[Guptajakala]

@RamadanAhmed Hi, have you tested out? How does it compare with the speed of Ceres?