util/lua/pointcloud_utils.lua

require 'csvigo'
require 'image'
local pl = require 'pl.import_into' ()
cv = require 'cv'
require 'cv.imgproc'

local utils = {}

function utils.transform(transformation, points)
   if points:dim() == 1 then
      points = points:repeatTensor(1, 1)
   end
   points = torch.cat(points, torch.ones(points:size(1)), 2)
   local newPoints = (transformation * points:transpose(1, 2)):transpose(1, 2)
   newPoints = torch.cdiv(newPoints[{{}, {1, 3}}], newPoints[{{}, {4}}]:expand(newPoints:size(1), 3))
   return newPoints
end

function utils.project(transformation, points)
   if points:dim() == 1 then
      points = points:repeatTensor(1, 1)
   end
   points = torch.cat(points, torch.ones(points:size(1)), 2)
   local points2D = (transformation * points:transpose(1, 2)):transpose(1, 2)
   points2D = torch.cdiv(points2D[{{}, {1, 2}}], points2D[{{}, {3}}]:expand(points2D:size(1), 2))
   return points2D:squeeze()
end

function utils.unproject(transformation, points2D, additionalConstraint)
   if points2D:dim() == 1 then
      points2D = points2D:repeatTensor(1, 1)
   end
   points2D = torch.cat(points2D, torch.ones(points2D:size(1)), 2)
   local A = torch.cat(transformation, additionalConstraint, 1)
   local b = torch.cat(points2D, torch.zeros(points2D:size(1), additionalConstraint:size(1)), 2)

   local points = (torch.inverse(A) * b:transpose(1, 2)):transpose(1, 2)
   points = torch.cdiv(points[{{}, {1, 3}}], points[{{}, {4}}]:expand(points:size(1), 3))
   return points:squeeze()
end

-- function utils.fitPlanes(points, numPlanes = 100, numIterations = 20)
--    local numPoints = points:size(1)
--    local numSampledPoints = 10000
--    local sampledPoints = points:index(1, perm:narrow(1, 1, numSampledPoints):long())

--    local perm = torch.randperm(numSampledPoints)
--    local indices = perm:narrow(1, 1, numIterations * 3):long()
--    local selectedPoints = sampledPoints:index(1, indices):reshape(numIterations, 3, 3)
--    local vectors_1 = selectedPoints[{{}, 1, {}}] - selectedPoints[{{}, 2, {}}]
--    local vectors_2 = selectedPoints[{{}, 1, {}}] - selectedPoints[{{}, 3, {}}]
--    local normals = torch.zeros(vectors_1:size())
--    normals[{{}, 1}] = vectors_1[{{}, 2}] * vectors_2[{{}, 3}] - vectors_1[{{}, 3}] * vectors_2[{{}, 2}]
--    normals[{{}, 2}] = vectors_1[{{}, 3}] * vectors_2[{{}, 1}] - vectors_1[{{}, 1}] * vectors_2[{{}, 3}]
--    normals[{{}, 3}] = vectors_1[{{}, 1}] * vectors_2[{{}, 2}] - vectors_1[{{}, 2}] * vectors_2[{{}, 1}]
--    local mean = torch.mean(selectedPoints, 2):squeeze()
--    local planeD = torch.sum(torch.cmul(normals, mean), 2)
--    local planes = normals * planeD:repeatTensor(1, 3)
-- end

function utils.projectLines(transformation, lines)
   local lines2D
   for lineIndex = 1, lines:size(1) do
      local line = lines[lineIndex]
      local points = torch.cat(line, torch.ones(2), 2)
      local points2D = (transformation * points:transpose(1, 2)):transpose(1, 2)
      points2D[{{}, 1}] = torch.cdiv(points2D[{{}, 1}], points2D[{{}, 3}])
      points2D[{{}, 2}] = torch.cdiv(points2D[{{}, 2}], points2D[{{}, 3}])
      local lineExists = false
      for pointIndex = 1, 2 do
         local point2D = points2D[pointIndex]
         --if point2D[3] > 0 and point2D[1] >= 1 and point2D[1] <= width and point2D[2] >= 1 and point2D[2] <= height then
         if point2D[3] > 0 then
            lineExists = true
         end
      end
      lineExists = true
      local line2D = torch.zeros(1, 2, 2)
      if lineExists then
         line2D = points2D[{{}, {1, 2}}]:repeatTensor(1, 1, 1)
      end
      if not lines2D then
         lines2D = line2D
      else
         lines2D = torch.cat(lines2D, line2D, 1)
      end
   end
   return lines2D
end

function utils.loadPointCloud(filename)
   local representationExists, representationInfo = pcall(function()
         return csvigo.load({path=filename, mode="large", header=false, separator=' ', verbose=false})
   end)
   local points = {}
   if representationExists and representationInfo ~= nil then
      local numPoints = tonumber(representationInfo[1][3])
      for pointIndex, point in pairs(representationInfo) do
	 if pointIndex >= 3 then
	    table.insert(points, {point[2], point[3], point[4]})
	 end
	 if pointIndex - 2 == numPoints then
	    break
	 end
      end
   end
   return torch.Tensor(points)
end

function utils.samplePoints(points, numSampledPoints)
   local indices = torch.randperm(points:size(1)):narrow(1, 1, numSampledPoints):long()
   local sampledPoints = points:index(1, indices)
   return sampledPoints
end

function utils.drawTopDownView(width, height, points, angle, transformation)
   local X = points[{{}, 1}]
   local Y = points[{{}, 2}]
   local points2D = torch.cat(X, Y, 2)

   local mean = torch.mean(points2D, 1)
   points2D = points2D - mean:expandAs(points2D)
   points2D:div(math.sqrt(points2D:size(1) - 1))

   if not transformation then
      if not angle then
      --local w, _, _ = torch.svd(points2D:t())
      --angle = torch.atan2(w[1][2], w[1][1])
      angle = 0
      end

      local newX = X * torch.cos(angle) + Y * torch.sin(angle)
      local newY = X * torch.sin(angle) - Y * torch.cos(angle)
      local newPoints2D = torch.cat(newX, newY, 2)

      local mins = torch.min(newPoints2D, 1)[1]
      local maxs = torch.max(newPoints2D, 1)[1]
      local paddingRatio = 0.05
      local padding = (maxs - mins) * paddingRatio
      mins = mins - padding
      maxs = maxs + padding

      local scaleFactor = math.min(width / (maxs[1] - mins[1]), height / (maxs[2] - mins[2]))
      transformation = torch.zeros(3, 4)
      transformation[1][1] = torch.cos(angle)
      transformation[1][2] = torch.sin(angle)
      transformation[1][4] = -mins[1]
      transformation[2][1] = torch.sin(angle)
      transformation[2][2] = -torch.cos(angle)
      transformation[2][4] = -mins[2]
      transformation[3][4] = 1
      transformation[1] = transformation[1] * scaleFactor
      transformation[2] = transformation[2] * scaleFactor
      --transformation[1], transformation[2] = transformation[2], transformation[1]\
      
   end

   local uv = utils.project(transformation, points)
   local topDownView = torch.zeros(height, width)
   for i = 1, uv:size(1) do
      local point = uv[i]
      topDownView[math.min(math.max(point[2], 1), height)][math.min(math.max(point[1], 1), width)] = topDownView[math.min(math.max(point[2], 1), height)][math.min(math.max(point[1], 1), width)] + 1
   end
   --image.save('test/pointcloud.png', topDownView)
   local pointDensity = 1
   topDownView = topDownView / topDownView:max()
--   topDownView[topDownView:gt(1)] = 1

   topDownView = torch.repeatTensor(topDownView, 3, 1, 1)
   return topDownView, transformation
end

function utils.getRotationMatrix(quaternion)
   qi = quaternion[1]
   qj = quaternion[2]
   qk = quaternion[3]
   qr = quaternion[4]
   local rotation = torch.zeros(4, 4)
   rotation[1][1] = 1 - 2 * (qj^2 + qk^2)
   rotation[1][2] = 2 * (qi * qj - qk * qr)
   rotation[1][3] = 2 * (qi * qk + qj * qr)
   rotation[2][1] = 2 * (qi * qj + qk * qr)
   rotation[2][2] = 1 - 2 * (qi^2 + qk^2)
   rotation[2][3] = 2 * (qj * qk - qi * qr)
   rotation[3][1] = 2 * (qi * qk - qj * qr)
   rotation[3][2] = 2 * (qj * qk + qi * qr)
   rotation[3][3] = 1 - 2 * (qi^2 + qj^2)
   rotation[4][4] = 1
   return rotation
end

function utils.getTransformation(camera, pose, orientation)
   local K = torch.zeros(3, 3)
   K[1][2] = 1
   K[2][1] = -1
   K[3][3] = 1
   local intrinsics = camera.intrinsics:clone()

   if not orientation or orientation == 1 then
      intrinsics[{{}, 1}] = -intrinsics[{{}, 1}]
      intrinsics[{{}, 3}] = -intrinsics[{{}, 3}]
   elseif orientation == 2 then
      intrinsics[{{}, 3}] = -intrinsics[{{}, 3}]
      local temp = intrinsics[{{}, 1}]:clone()
      intrinsics[{{}, 1}] = -intrinsics[{{}, 2}]
      intrinsics[{{}, 2}] = -temp
      local temp = intrinsics[1][3]
      intrinsics[1][3] = intrinsics[2][3]
      intrinsics[2][3] = temp
   end

   --intrinsics[2][2] = -intrinsics[2][2]
   intrinsics = torch.cat(intrinsics, torch.zeros(3), 2)

   local inverseQuaternion = pose[{{4, 7}}]:clone()
   inverseQuaternion[{{1, 3}}] = -inverseQuaternion[{{1, 3}}]
   local rotation = utils.getRotationMatrix(inverseQuaternion)
   local translation = torch.zeros(4, 4)
   translation[1][1] = 1
   translation[2][2] = 1
   translation[3][3] = 1
   translation[1][4] = -pose[1]
   translation[2][4] = -pose[2]
   translation[3][4] = -pose[3]
   translation[4][4] = 1
   local transformation = intrinsics * rotation * translation
   print('frame')
   print(pose)
   print(intrinsics)
   -- print(K)
   print(rotation)
   print(translation)
   print(transformation)

   local point = torch.Tensor({-0.0527, 2.0441, 1.000, 1})
   -- print(point)
   -- print(rotation * point)
   -- print(translation * rotation * point)
   -- print(transformation * point)
   return transformation
end

-- function utils.getRotationFromAngle(angle)
--    rotation[1][1] = torch.cos(angle)
--    rotation[1][2] = torch.sin(angle)
--    rotation[2][1] = torch.sin(angle)
--    rotation[2][2] = -torch.cos(angle)
--    rotation[3][3] = 1
--    return rotation
-- end

return utils