diff --git a/.gitmodules b/.gitmodules index 6fd42fbd..cc3b441b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,6 @@ [submodule "third-party/Eigen"] path = third-party/Eigen url = https://github.com/eigenteam/eigen-git-mirror.git +[submodule "third-party/Sophus"] + path = third-party/Sophus + url = https://github.com/strasdat/Sophus.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 9ef4b20c..292a5b8f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,7 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/CMakeMod set(Pangolin_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third-party/Pangolin/build/src" CACHE PATH "Pangolin build directory") set(OPENNI2_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third-party/OpenNI2/Include" CACHE PATH "OpenNI2 Include directory") set(OPENNI2_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/third-party/OpenNI2/Bin/x64-Release/libOpenNI2.so" CACHE PATH "OpenNI2 library") +set(Sophus_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third-party/Sophus" CACHE PATH "Sophus Include directory") set(efusion_SHADER_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Core/Shaders" CACHE PATH "Where the shaders live") find_package(LAPACK REQUIRED) @@ -25,6 +26,7 @@ include_directories(${Pangolin_INCLUDE_DIRS}) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${OPENNI2_INCLUDE_DIR}) include_directories(${SUITESPARSE_INCLUDE_DIRS}) +include_directories(${Sophus_INCLUDE_DIR}) if(WITH_REALSENSE) include_directories(${REALSENSE_INCLUDE_DIR}) @@ -36,6 +38,8 @@ file(GLOB srcs *.cpp *.h *.cu *.cuh) file(GLOB tools_srcs Tools/*.cpp Tools/*.h Tools/*.cu Tools/*.cuh) add_definitions(-Dlinux=1) +add_definitions(-DEIGEN_MAX_ALIGN_BYTES=0) +add_definitions(-DEIGEN_MAX_STATIC_ALIGN_BYTES=0) set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-O3 -msse2 -msse3 -Wall -std=c++11 -DSHADER_DIR=${efusion_SHADER_DIR}") #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -Wall -std=c++11 -DSHADER_DIR=${efusion_SHADER_DIR}") diff --git a/Core/CMakeLists.txt b/Core/CMakeLists.txt index 0c087a16..a0b5cf59 100644 --- a/Core/CMakeLists.txt +++ b/Core/CMakeLists.txt @@ -25,7 +25,7 @@ endforeach(NVCC_ARCH) include("${CMAKE_MODULE_PATH}/CudaComputeTargetFlags.cmake") APPEND_TARGET_ARCH_FLAGS() -set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-Xcompiler;-fPIC;") +set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-Xcompiler;-fPIC;--expt-relaxed-constexpr") CUDA_COMPILE(cuda_objs ${cuda}) diff --git a/Core/Cuda/cudafuncs.cuh b/Core/Cuda/cudafuncs.cuh index 34cbc493..293cb01a 100644 --- a/Core/Cuda/cudafuncs.cuh +++ b/Core/Cuda/cudafuncs.cuh @@ -53,12 +53,6 @@ #ifndef CUDA_CUDAFUNCS_CUH_ #define CUDA_CUDAFUNCS_CUH_ -#if __CUDA_ARCH__ < 300 -#define MAX_THREADS 512 -#else -#define MAX_THREADS 1024 -#endif - #include #include #include "containers/device_array.hpp" diff --git a/Core/Cuda/estimate.cu b/Core/Cuda/estimate.cu new file mode 100644 index 00000000..a631dac4 --- /dev/null +++ b/Core/Cuda/estimate.cu @@ -0,0 +1,247 @@ +#include "convenience.cuh" +#include "estimate.cuh" + +template +__inline__ __device__ void warpReduceSum(Eigen::Matrix& val) { + for (int offset = warpSize / 2; offset > 0; offset /= 2) { +#pragma unroll + for (int i = 0; i < D; i++) { + val[i] += __shfl_down_sync(0xFFFFFFFF, val[i], offset); + } + } +} + +template +__inline__ __device__ void blockReduceSum(Eigen::Matrix& val) { + // Allocate shared memory in two steps otherwise NVCC complains about Eigen's + // non-empty constructor + static __shared__ unsigned char sharedMem[32 * sizeof(Eigen::Matrix)]; + + Eigen::Matrix(&shared)[32] = + reinterpret_cast(&)[32]>(sharedMem); + + int lane = threadIdx.x % warpSize; + + int wid = threadIdx.x / warpSize; + + warpReduceSum(val); + + // write reduced value to shared memory + if (lane == 0) { + shared[wid] = val; + } + __syncthreads(); + + // ensure we only grab a value from shared memory if that warp existed + val = (threadIdx.x < blockDim.x / warpSize) ? shared[lane] : Eigen::Matrix::Zero(); + + if (wid == 0) { + warpReduceSum(val); + } +} + +template +__global__ void reduceSum(Eigen::Matrix* in, Eigen::Matrix* out, int N) { + Eigen::Matrix sum = Eigen::Matrix::Zero(); + + for (int i = blockIdx.x * blockDim.x + threadIdx.x; i < N; i += blockDim.x * gridDim.x) { + sum += in[i]; + } + + blockReduceSum(sum); + + if (threadIdx.x == 0) { + out[blockIdx.x] = sum; + } +} + +struct Reduction { + Eigen::Matrix3f R_prev_curr; + Eigen::Vector3f t_prev_curr; + + CameraModel intr; + + PtrStep vmap_curr; + PtrStep nmap_curr; + + PtrStep vmap_prev; + PtrStep nmap_prev; + + float dist_thresh; + float angle_thresh; + + int cols; + int rows; + int N; + + Eigen::Matrix* out; + + // And now for some template metaprogramming magic + template + struct SquareUpperTriangularProduct { + __device__ __forceinline__ static void apply( + Eigen::Matrix& values, + const float (&rows)[end + 1]) { + values[((end + 1) * outer) + inner - (outer * (outer + 1) / 2)] = rows[outer] * rows[inner]; + + SquareUpperTriangularProduct::apply(values, rows); + } + }; + + // Inner loop base + template + struct SquareUpperTriangularProduct { + __device__ __forceinline__ static void apply( + Eigen::Matrix& values, + const float (&rows)[end + 1]) { + values[((end + 1) * outer) + end - (outer * (outer + 1) / 2)] = rows[outer] * rows[end]; + + SquareUpperTriangularProduct::apply(values, rows); + } + }; + + // Outer loop base + template + struct SquareUpperTriangularProduct { + __device__ __forceinline__ static void apply( + Eigen::Matrix& values, + const float (&rows)[end + 1]) { + values[((end + 1) * end) + end - (end * (end + 1) / 2)] = rows[end] * rows[end]; + } + }; + + __device__ __forceinline__ void operator()() const { + Eigen::Matrix sum = Eigen::Matrix::Zero(); + + SquareUpperTriangularProduct<0, 0, 6> sutp; + + Eigen::Matrix values; + + for (int i = blockIdx.x * blockDim.x + threadIdx.x; i < N; i += blockDim.x * gridDim.x) { + const int y = i / cols; + const int x = i - (y * cols); + + const Eigen::Vector3f v_curr( + vmap_curr.ptr(y)[x], vmap_curr.ptr(y + rows)[x], vmap_curr.ptr(y + 2 * rows)[x]); + + const Eigen::Vector3f v_curr_in_prev = R_prev_curr * v_curr + t_prev_curr; + + const Eigen::Matrix p_curr_in_prev( + __float2int_rn(v_curr_in_prev(0) * intr.fx / v_curr_in_prev(2) + intr.cx), + __float2int_rn(v_curr_in_prev(1) * intr.fy / v_curr_in_prev(2) + intr.cy)); + + float row[7] = {0, 0, 0, 0, 0, 0, 0}; + + values[28] = 0; + + if (p_curr_in_prev(0) >= 0 && p_curr_in_prev(1) >= 0 && p_curr_in_prev(0) < cols && + p_curr_in_prev(1) < rows && v_curr(2) > 0 && v_curr_in_prev(2) > 0) { + const Eigen::Vector3f v_prev( + vmap_prev.ptr(p_curr_in_prev(1))[p_curr_in_prev(0)], + vmap_prev.ptr(p_curr_in_prev(1) + rows)[p_curr_in_prev(0)], + vmap_prev.ptr(p_curr_in_prev(1) + 2 * rows)[p_curr_in_prev(0)]); + + const Eigen::Vector3f n_curr( + nmap_curr.ptr(y)[x], nmap_curr.ptr(y + rows)[x], nmap_curr.ptr(y + 2 * rows)[x]); + + const Eigen::Vector3f n_curr_in_prev = R_prev_curr * n_curr; + + const Eigen::Vector3f n_prev( + nmap_prev.ptr(p_curr_in_prev(1))[p_curr_in_prev(0)], + nmap_prev.ptr(p_curr_in_prev(1) + rows)[p_curr_in_prev(0)], + nmap_prev.ptr(p_curr_in_prev(1) + 2 * rows)[p_curr_in_prev(0)]); + + if (n_curr_in_prev.cross(n_prev).norm() < angle_thresh && + (v_prev - v_curr_in_prev).norm() < dist_thresh && !isnan(n_curr(0)) && + !isnan(n_prev(0))) { + *(Eigen::Vector3f*)&row[0] = n_prev; + *(Eigen::Vector3f*)&row[3] = v_curr_in_prev.cross(n_prev); + row[6] = n_prev.dot(v_prev - v_curr_in_prev); + + values[28] = 1; + + sutp.apply(values, row); + + sum += values; + } + } + } + + blockReduceSum(sum); + + if (threadIdx.x == 0) { + out[blockIdx.x] = sum; + } + } +}; + +__global__ void estimateKernel(const Reduction reduction) { + reduction(); +} + +void estimateStep( + const Eigen::Matrix3f& R_prev_curr, + const Eigen::Vector3f& t_prev_curr, + const DeviceArray2D& vmap_curr, + const DeviceArray2D& nmap_curr, + const CameraModel& intr, + const DeviceArray2D& vmap_prev, + const DeviceArray2D& nmap_prev, + float dist_thresh, + float angle_thresh, + DeviceArray>& sum, + DeviceArray>& out, + float* matrixA_host, + float* vectorB_host, + float* residual_inliers) { + int cols = vmap_curr.cols(); + int rows = vmap_curr.rows() / 3; + + Reduction reduction; + + reduction.R_prev_curr = R_prev_curr; + reduction.t_prev_curr = t_prev_curr; + + reduction.vmap_curr = vmap_curr; + reduction.nmap_curr = nmap_curr; + + reduction.intr = intr; + + reduction.vmap_prev = vmap_prev; + reduction.nmap_prev = nmap_prev; + + reduction.dist_thresh = dist_thresh; + reduction.angle_thresh = angle_thresh; + + reduction.cols = cols; + reduction.rows = rows; + + reduction.N = cols * rows; + reduction.out = sum; + + estimateKernel<<>>(reduction); + + reduceSum<29><<<1, MAX_THREADS>>>(sum, out, reduceBlocks); + + cudaSafeCall(cudaGetLastError()); + cudaSafeCall(cudaDeviceSynchronize()); + + float host_data[29]; + out.download((Eigen::Matrix*)&host_data[0]); + + int shift = 0; + for (int i = 0; i < 6; ++i) // rows + { + for (int j = i; j < 7; ++j) // cols + b + { + float value = host_data[shift++]; + if (j == 6) // vector b + vectorB_host[i] = value; + else + matrixA_host[j * 6 + i] = matrixA_host[i * 6 + j] = value; + } + } + + residual_inliers[0] = host_data[27]; + residual_inliers[1] = host_data[28]; +} diff --git a/Core/Cuda/estimate.cuh b/Core/Cuda/estimate.cuh new file mode 100644 index 00000000..0556a604 --- /dev/null +++ b/Core/Cuda/estimate.cuh @@ -0,0 +1,61 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "containers/device_array.hpp" +#include "types.cuh" + +#include +#include +#include + +void estimateStep( + const Eigen::Matrix3f& R_prev_curr, + const Eigen::Vector3f& t_prev_curr, + const DeviceArray2D& vmap_curr, + const DeviceArray2D& nmap_curr, + const CameraModel& intr, + const DeviceArray2D& vmap_prev, + const DeviceArray2D& nmap_prev, + float dist_thresh, + float angle_thresh, + DeviceArray>& sum, + DeviceArray>& out, + float* matrixA_host, + float* vectorB_host, + float* residual_inliers); diff --git a/Core/Cuda/reduce.cu b/Core/Cuda/reduce.cu index a7f1ec9e..5d70c3be 100644 --- a/Core/Cuda/reduce.cu +++ b/Core/Cuda/reduce.cu @@ -54,9 +54,6 @@ #include "cudafuncs.cuh" #include "operators.cuh" -constexpr int reduceThreads = 256; -constexpr int reduceBlocks = 64; - __inline__ __device__ JtJJtrSE3 warpReduceSum(JtJJtrSE3 val) { for (int offset = warpSize / 2; offset > 0; offset /= 2) { val.aa += __shfl_down_sync(0xFFFFFFFF, val.aa, offset); diff --git a/Core/Cuda/types.cuh b/Core/Cuda/types.cuh index 6aaa36c5..9d2597dd 100644 --- a/Core/Cuda/types.cuh +++ b/Core/Cuda/types.cuh @@ -59,6 +59,11 @@ #include #endif +#define MAX_THREADS 1024 + +constexpr int reduceThreads = 256; +constexpr int reduceBlocks = 64; + struct mat33 { mat33() {} diff --git a/Core/Deformation.cpp b/Core/Deformation.cpp index 607f4d71..1b443002 100644 --- a/Core/Deformation.cpp +++ b/Core/Deformation.cpp @@ -26,16 +26,17 @@ Deformation::Deformation() sampleProgram(loadProgramGeomFromFile("sample.vert", "sample.geom")), bufferSize(1024), // max nodes basically count(0), - vertices(new Eigen::Vector4f[bufferSize]), - graphPosePoints(new std::vector), + rawSampledNodes_w(new Eigen::Vector4f[bufferSize]), + graphPosePoints(new std::vector), lastDeformTime(0) { // x, y, z and init time - memset(&vertices[0], 0, bufferSize); + memset(&rawSampledNodes_w[0], 0, bufferSize); glGenTransformFeedbacks(1, &fid); glGenBuffers(1, &vbo); glBindBuffer(GL_ARRAY_BUFFER, vbo); - glBufferData(GL_ARRAY_BUFFER, bufferSize * sizeof(Eigen::Vector4f), &vertices[0], GL_STREAM_DRAW); + glBufferData( + GL_ARRAY_BUFFER, bufferSize * sizeof(Eigen::Vector4f), &rawSampledNodes_w[0], GL_STREAM_DRAW); glBindBuffer(GL_ARRAY_BUFFER, 0); sampleProgram->Bind(); @@ -52,7 +53,7 @@ Deformation::Deformation() } Deformation::~Deformation() { - delete[] vertices; + delete[] rawSampledNodes_w; glDeleteTransformFeedbacks(1, &fid); glDeleteBuffers(1, &vbo); glDeleteQueries(1, &countQuery); @@ -68,8 +69,8 @@ void Deformation::addConstraint(const Constraint& constraint) { } void Deformation::addConstraint( - const Eigen::Vector4f& src, - const Eigen::Vector4f& target, + const Eigen::Vector4d& src, + const Eigen::Vector4d& target, const uint64_t& srcTime, const uint64_t& targetTime, const bool pinConstraints) { @@ -87,31 +88,31 @@ bool Deformation::constrain( std::vector& rawGraph, int time, const bool fernMatch, - std::vector>& poseGraph, + std::vector>& t_T_wc, const bool relaxGraph, std::vector* newRelativeCons) { if (def.isInit()) { - std::vector times; - std::vector poses; - std::vector rawPoses; + std::vector times; + std::vector T_wcs; + std::vector T_wc_ptrs; // Deform the set of ferns for (size_t i = 0; i < ferns.size(); i++) { times.push_back(ferns.at(i)->srcTime); - poses.push_back(ferns.at(i)->pose); - rawPoses.push_back(&ferns.at(i)->pose); + T_wcs.push_back(ferns.at(i)->T_wc); + T_wc_ptrs.push_back(&ferns.at(i)->T_wc); } if (fernMatch) { // Also apply to the current full pose graph (this might be silly :D) - for (size_t i = 0; i < poseGraph.size(); i++) { - times.push_back(poseGraph.at(i).first); - poses.push_back(poseGraph.at(i).second); - rawPoses.push_back(&poseGraph.at(i).second); + for (size_t i = 0; i < t_T_wc.size(); i++) { + times.push_back(t_T_wc.at(i).first); + T_wcs.push_back(t_T_wc.at(i).second); + T_wc_ptrs.push_back(&t_T_wc.at(i).second); } } - def.setPosesSeq(×, poses); + def.setPosesSeq(×, T_wcs); int originalPointPool = pointPool.size(); @@ -136,7 +137,7 @@ bool Deformation::constrain( def.addRelativeConstraint( constraints.at(i).srcPointPoolId, constraints.at(i).tarPointPoolId); } else { - Eigen::Vector3f targetPoint = constraints.at(i).target; + Eigen::Vector3d targetPoint = constraints.at(i).target; def.addConstraint(constraints.at(i).srcPointPoolId, targetPoint); } } @@ -149,7 +150,7 @@ bool Deformation::constrain( bool poseUpdated = false; if (!fernMatch || (fernMatch && optimised && meanConsError < 0.0003 && error < 0.12)) { - def.applyGraphToPoses(rawPoses); + def.applyGraphToPoses(T_wc_ptrs); def.applyGraphToVertices(); @@ -170,15 +171,18 @@ bool Deformation::constrain( } std::vector& graphNodes = def.getGraph(); - std::vector graphTimes = def.getGraphTimes(); + std::vector graphTimes = def.getGraphTimes(); // 16 floats per node... rawGraph.resize(graphNodes.size() * 16); for (size_t i = 0; i < graphNodes.size(); i++) { - memcpy(&rawGraph.at(i * 16), graphNodes.at(i)->position.data(), sizeof(float) * 3); - memcpy(&rawGraph.at(i * 16 + 3), graphNodes.at(i)->rotation.data(), sizeof(float) * 9); - memcpy(&rawGraph.at(i * 16 + 12), graphNodes.at(i)->translation.data(), sizeof(float) * 3); + const Eigen::Vector3f position = graphNodes.at(i)->position.cast(); + const Eigen::Matrix3f rotation = graphNodes.at(i)->rotation.cast(); + const Eigen::Vector3f translation = graphNodes.at(i)->translation.cast(); + memcpy(&rawGraph.at(i * 16), position.data(), sizeof(float) * 3); + memcpy(&rawGraph.at(i * 16 + 3), rotation.data(), sizeof(float) * 9); + memcpy(&rawGraph.at(i * 16 + 12), translation.data(), sizeof(float) * 3); rawGraph.at(i * 16 + 15) = (float)graphTimes.at(i); } @@ -201,21 +205,19 @@ bool Deformation::constrain( } void Deformation::sampleGraphFrom(Deformation& other) { - Eigen::Vector4f* otherVerts = other.getVertices(); + Eigen::Vector4f* otherRawSampledNodes_w = other.getRawSampledNodes_w(); - int sampleRate = 5; + constexpr int sampleRate = 5; if (other.getCount() / sampleRate > def.k) { for (int i = 0; i < other.getCount(); i += sampleRate) { - Eigen::Vector3f newPoint = otherVerts[i].head<3>(); + graphPosePoints->push_back(otherRawSampledNodes_w[i].head<3>().cast()); - graphPosePoints->push_back(newPoint); - - if (i > 0 && otherVerts[i](3) < graphPoseTimes.back()) { + if (i > 0 && otherRawSampledNodes_w[i](3) < graphPoseTimes.back()) { assert(false && "Assumption failed"); } - graphPoseTimes.push_back(otherVerts[i](3)); + graphPoseTimes.push_back(otherRawSampledNodes_w[i](3)); } def.initialiseGraph(graphPosePoints, &graphPoseTimes); @@ -280,20 +282,18 @@ void Deformation::sampleGraphModel(const std::pair& model) { if ((int)count > def.k) { glBindBuffer(GL_ARRAY_BUFFER, vbo); - glGetBufferSubData(GL_ARRAY_BUFFER, 0, sizeof(Eigen::Vector4f) * count, vertices); + glGetBufferSubData(GL_ARRAY_BUFFER, 0, sizeof(Eigen::Vector4f) * count, rawSampledNodes_w); glBindBuffer(GL_ARRAY_BUFFER, 0); for (size_t i = 0; i < count; i++) { - Eigen::Vector3f newPoint = vertices[i].head<3>(); - - graphPosePoints->push_back(newPoint); + graphPosePoints->push_back(rawSampledNodes_w[i].head<3>().cast()); - if (i > 0 && vertices[i](3) < graphPoseTimes.back()) { + if (i > 0 && rawSampledNodes_w[i](3) < graphPoseTimes.back()) { assert(false && "Assumption failed"); } - graphPoseTimes.push_back(vertices[i](3)); + graphPoseTimes.push_back(rawSampledNodes_w[i](3)); } def.initialiseGraph(graphPosePoints, &graphPoseTimes); diff --git a/Core/Deformation.h b/Core/Deformation.h index d6a981d1..8f67b34b 100644 --- a/Core/Deformation.h +++ b/Core/Deformation.h @@ -47,8 +47,8 @@ class Deformation { class Constraint { public: Constraint( - const Eigen::Vector3f& src, - const Eigen::Vector3f& target, + const Eigen::Vector3d& src, + const Eigen::Vector3d& target, const uint64_t& srcTime, const uint64_t& targetTime, const bool relative, @@ -62,8 +62,8 @@ class Deformation { srcPointPoolId(-1), tarPointPoolId(-1) {} - Eigen::Vector3f src; - Eigen::Vector3f target; + Eigen::Vector3d src; + Eigen::Vector3d target; uint64_t srcTime; uint64_t targetTime; bool relative; @@ -73,8 +73,8 @@ class Deformation { }; void addConstraint( - const Eigen::Vector4f& src, - const Eigen::Vector4f& target, + const Eigen::Vector4d& src, + const Eigen::Vector4d& target, const uint64_t& srcTime, const uint64_t& targetTime, const bool pinConstraints); @@ -86,12 +86,12 @@ class Deformation { std::vector& rawGraph, int time, const bool fernMatch, - std::vector>& poseGraph, + std::vector>& t_T_wc, const bool relaxGraph, std::vector* newRelativeCons = 0); - Eigen::Vector4f* getVertices() { - return vertices; + Eigen::Vector4f* getRawSampledNodes_w() { + return rawSampledNodes_w; } int getCount() { @@ -105,8 +105,8 @@ class Deformation { private: DeformationGraph def; - std::vector vertexTimes; - std::vector pointPool; + std::vector vertexTimes; + std::vector pointPool; int originalPointPool; int firstGraphNode; @@ -116,11 +116,11 @@ class Deformation { const int bufferSize; GLuint countQuery; uint32_t count; - Eigen::Vector4f* vertices; + Eigen::Vector4f* rawSampledNodes_w; std::vector> poseGraphPoints; - std::vector graphPoseTimes; - std::vector* graphPosePoints; + std::vector graphPoseTimes; + std::vector* graphPosePoints; std::vector constraints; int lastDeformTime; diff --git a/Core/ElasticFusion.cpp b/Core/ElasticFusion.cpp index 2dbcd423..471cd723 100644 --- a/Core/ElasticFusion.cpp +++ b/Core/ElasticFusion.cpp @@ -52,7 +52,6 @@ ElasticFusion::ElasticFusion( Intrinsics::getInstance().fy()), ferns(500, depthCut * 1000, photoThresh), saveFilename(fileName), - currPose(Eigen::Matrix4f::Identity()), tick(1), timeDelta(timeDelta), icpCountThresh(countThresh), @@ -117,7 +116,7 @@ ElasticFusion::~ElasticFusion() { std::ofstream f; f.open(fname.c_str(), std::fstream::out); - for (size_t i = 0; i < poseGraph.size(); i++) { + for (size_t i = 0; i < t_T_wc.size(); i++) { std::stringstream strs; if (iclnuim) { @@ -126,12 +125,12 @@ ElasticFusion::~ElasticFusion() { strs << std::setprecision(6) << std::fixed << (double)poseLogTimes.at(i) / 1000000.0 << " "; } - Eigen::Vector3f trans = poseGraph.at(i).second.topRightCorner(3, 1); - Eigen::Matrix3f rot = poseGraph.at(i).second.topLeftCorner(3, 3); + const Eigen::Vector3d trans = t_T_wc.at(i).second.translation(); + const Eigen::Matrix3d rot = t_T_wc.at(i).second.rotationMatrix(); f << strs.str() << trans(0) << " " << trans(1) << " " << trans(2) << " "; - Eigen::Quaternionf currentCameraRotation(rot); + Eigen::Quaterniond currentCameraRotation(rot); f << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n"; @@ -272,9 +271,8 @@ void ElasticFusion::processFrame( const uint8_t* rgb, const uint16_t* depth, const int64_t& timestamp, - const Eigen::Matrix4f* inPose, const float weightMultiplier, - const bool bootstrap) { + const Sophus::SE3d* in_T_wc) { TICK("Run"); textures[GPUTexture::DEPTH_RAW]->texture->Upload( @@ -297,11 +295,11 @@ void ElasticFusion::processFrame( frameToModel.initFirstRGB(textures[GPUTexture::RGB]); } else { - Eigen::Matrix4f lastPose = currPose; + const Sophus::SE3d T_wc_prev = T_wc_curr; bool trackingOk = true; - if (bootstrap || !inPose) { + if (!in_T_wc) { TICK("autoFill"); resize.image(indexMap.imageTex(), imageBuff); bool shouldFillIn = !denseEnough(imageBuff); @@ -312,7 +310,7 @@ void ElasticFusion::processFrame( frameToModel.initICPModel( shouldFillIn ? &fillIn.vertexTexture : indexMap.vertexTex(), shouldFillIn ? &fillIn.normalTexture : indexMap.normalTex(), - currPose); + T_wc_curr); frameToModel.initRGBModel( (shouldFillIn || frameToFrameRGB) ? &fillIn.imageTexture : indexMap.imageTex()); @@ -320,17 +318,9 @@ void ElasticFusion::processFrame( frameToModel.initRGB(textures[GPUTexture::RGB]); TOCK("odomInit"); - if (bootstrap) { - assert(inPose); - currPose = currPose * (*inPose); - } - - Eigen::Vector3f trans = currPose.topRightCorner(3, 1); - Eigen::Matrix rot = currPose.topLeftCorner(3, 3); - TICK("odom"); frameToModel.getIncrementalTransformation( - trans, rot, rgbOnly, icpWeight, pyramid, fastOdom, so3); + T_wc_curr, rgbOnly, icpWeight, pyramid, fastOdom, so3); TOCK("odom"); trackingOk = !reloc || frameToModel.lastICPError < 1e-04; @@ -374,19 +364,14 @@ void ElasticFusion::processFrame( } } - currPose.topRightCorner(3, 1) = trans; - currPose.topLeftCorner(3, 3) = rot; } else { - currPose = *inPose; + T_wc_curr = *in_T_wc; } - Eigen::Matrix4f diff = currPose.inverse() * lastPose; - - Eigen::Vector3f diffTrans = diff.topRightCorner(3, 1); - Eigen::Matrix3f diffRot = diff.topLeftCorner(3, 3); + const Sophus::SE3d T_curr_prev = T_wc_curr.inverse() * T_wc_prev; // Weight by velocity - float weighting = std::max(diffTrans.norm(), rodrigues2(diffRot).norm()); + float weighting = std::max(T_curr_prev.translation().norm(), T_curr_prev.log().norm()); float largest = 0.01; float minWeight = 0.5; @@ -401,15 +386,15 @@ void ElasticFusion::processFrame( predict(); - Eigen::Matrix4f recoveryPose = currPose; + Sophus::SE3d T_wc_recovery = T_wc_curr; if (closeLoops) { lastFrameRecovery = false; TICK("Ferns::findFrame"); - recoveryPose = ferns.findFrame( + T_wc_recovery = ferns.findFrame( constraints, - currPose, + T_wc_curr, &fillIn.vertexTexture, &fillIn.normalTexture, &fillIn.imageTexture, @@ -424,7 +409,7 @@ void ElasticFusion::processFrame( if (closeLoops && ferns.lastClosest != -1) { if (lost) { - currPose = recoveryPose; + T_wc_curr = T_wc_recovery; lastFrameRecovery = true; } else { for (size_t i = 0; i < constraints.size(); i++) { @@ -440,14 +425,14 @@ void ElasticFusion::processFrame( globalDeformation.addConstraint(relativeCons.at(i)); } - if (globalDeformation.constrain(ferns.frames, rawGraph, tick, true, poseGraph, true)) { - currPose = recoveryPose; + if (globalDeformation.constrain(ferns.frames, rawGraph, tick, true, t_T_wc, true)) { + T_wc_curr = T_wc_recovery; poseMatches.push_back(PoseMatch( ferns.lastClosest, ferns.frames.size(), - ferns.frames.at(ferns.lastClosest)->pose, - currPose, + ferns.frames.at(ferns.lastClosest)->T_wc, + T_wc_curr, constraints, true)); @@ -464,7 +449,7 @@ void ElasticFusion::processFrame( // failed!) TICK("IndexMap::INACTIVE"); indexMap.combinedPredict( - currPose, + T_wc_curr, globalModel.model(), maxDepthProcessed, confidenceThreshold, @@ -475,16 +460,15 @@ void ElasticFusion::processFrame( TOCK("IndexMap::INACTIVE"); // WARNING initICP* must be called before initRGB* - modelToModel.initICPModel(indexMap.oldVertexTex(), indexMap.oldNormalTex(), currPose); + modelToModel.initICPModel(indexMap.oldVertexTex(), indexMap.oldNormalTex(), T_wc_curr); modelToModel.initRGBModel(indexMap.oldImageTex()); modelToModel.initICP(indexMap.vertexTex(), indexMap.normalTex()); modelToModel.initRGB(indexMap.imageTex()); - Eigen::Vector3f trans = currPose.topRightCorner(3, 1); - Eigen::Matrix rot = currPose.topLeftCorner(3, 3); + Sophus::SE3d T_wc_est = T_wc_curr; - modelToModel.getIncrementalTransformation(trans, rot, false, 10, pyramid, fastOdom, false); + modelToModel.getIncrementalTransformation(T_wc_est, false, 10, pyramid, fastOdom, false); Eigen::MatrixXd covar = modelToModel.getCovariance(); bool covOk = true; @@ -496,11 +480,6 @@ void ElasticFusion::processFrame( } } - Eigen::Matrix4f estPose = Eigen::Matrix4f::Identity(); - - estPose.topRightCorner(3, 1) = trans; - estPose.topLeftCorner(3, 3) = rot; - if (covOk && modelToModel.lastICPCount > icpCountThresh && modelToModel.lastICPError < icpErrThresh) { resize.vertex(indexMap.vertexTex(), consBuff); @@ -511,22 +490,22 @@ void ElasticFusion::processFrame( if (consBuff.at(j, i)(2) > 0 && consBuff.at(j, i)(2) < maxDepthProcessed && timesBuff.at(j, i) > 0) { - Eigen::Vector4f worldRawPoint = currPose * - Eigen::Vector4f(consBuff.at(j, i)(0), + Eigen::Vector4d vert_w_curr = T_wc_curr * + Eigen::Vector4d(consBuff.at(j, i)(0), consBuff.at(j, i)(1), consBuff.at(j, i)(2), - 1.0f); + 1.0); - Eigen::Vector4f worldModelPoint = estPose * - Eigen::Vector4f(consBuff.at(j, i)(0), + Eigen::Vector4d vert_w_est = T_wc_est * + Eigen::Vector4d(consBuff.at(j, i)(0), consBuff.at(j, i)(1), consBuff.at(j, i)(2), - 1.0f); + 1.0); - constraints.push_back(Ferns::SurfaceConstraint(worldRawPoint, worldModelPoint)); + constraints.push_back(Ferns::SurfaceConstraint(vert_w_curr, vert_w_est)); localDeformation.addConstraint( - worldRawPoint, worldModelPoint, tick, timesBuff.at(j, i), deforms == 0); + vert_w_curr, vert_w_est, tick, timesBuff.at(j, i), deforms == 0); } } } @@ -534,13 +513,18 @@ void ElasticFusion::processFrame( std::vector newRelativeCons; if (localDeformation.constrain( - ferns.frames, rawGraph, tick, false, poseGraph, false, &newRelativeCons)) { + ferns.frames, rawGraph, tick, false, t_T_wc, false, &newRelativeCons)) { poseMatches.push_back(PoseMatch( - ferns.frames.size() - 1, ferns.frames.size(), estPose, currPose, constraints, false)); + ferns.frames.size() - 1, + ferns.frames.size(), + T_wc_est, + T_wc_curr, + constraints, + false)); deforms += rawGraph.size() > 0; - currPose = estPose; + T_wc_curr = T_wc_est; for (size_t i = 0; i < newRelativeCons.size(); i += newRelativeCons.size() / 3) { relativeCons.push_back(newRelativeCons.at(i)); @@ -551,11 +535,11 @@ void ElasticFusion::processFrame( if (!rgbOnly && trackingOk && !lost) { TICK("indexMap"); - indexMap.predictIndices(currPose, tick, globalModel.model(), maxDepthProcessed, timeDelta); + indexMap.predictIndices(T_wc_curr, tick, globalModel.model(), maxDepthProcessed, timeDelta); TOCK("indexMap"); globalModel.fuse( - currPose, + T_wc_curr, tick, textures[GPUTexture::RGB], textures[GPUTexture::DEPTH_METRIC], @@ -565,11 +549,10 @@ void ElasticFusion::processFrame( indexMap.colorTimeTex(), indexMap.normalRadTex(), maxDepthProcessed, - confidenceThreshold, weighting); TICK("indexMap"); - indexMap.predictIndices(currPose, tick, globalModel.model(), maxDepthProcessed, timeDelta); + indexMap.predictIndices(T_wc_curr, tick, globalModel.model(), maxDepthProcessed, timeDelta); TOCK("indexMap"); // If we're deforming we need to predict the depth again to figure out which @@ -577,7 +560,7 @@ void ElasticFusion::processFrame( // this loop if (rawGraph.size() > 0 && !fernAccepted) { indexMap.synthesizeDepth( - currPose, + T_wc_curr, globalModel.model(), maxDepthProcessed, confidenceThreshold, @@ -587,7 +570,7 @@ void ElasticFusion::processFrame( } globalModel.clean( - currPose, + T_wc_curr, tick, indexMap.indexTex(), indexMap.vertConfTex(), @@ -602,7 +585,7 @@ void ElasticFusion::processFrame( } } - poseGraph.push_back(std::pair(tick, currPose)); + t_T_wc.push_back(std::pair(tick, T_wc_curr)); poseLogTimes.push_back(timestamp); TICK("sampleGraph"); @@ -629,7 +612,7 @@ void ElasticFusion::processFerns() { &fillIn.imageTexture, &fillIn.vertexTexture, &fillIn.normalTexture, - currPose, + T_wc_curr, tick, fernThresh); TOCK("Ferns::addFrame"); @@ -640,7 +623,7 @@ void ElasticFusion::predict() { if (lastFrameRecovery) { indexMap.combinedPredict( - currPose, + T_wc_curr, globalModel.model(), maxDepthProcessed, confidenceThreshold, @@ -650,7 +633,7 @@ void ElasticFusion::predict() { IndexMap::ACTIVE); } else { indexMap.combinedPredict( - currPose, + T_wc_curr, globalModel.model(), maxDepthProcessed, confidenceThreshold, @@ -797,50 +780,6 @@ void ElasticFusion::savePly() { delete[] mapData; } -Eigen::Vector3f ElasticFusion::rodrigues2(const Eigen::Matrix3f& matrix) { - Eigen::JacobiSVD svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU); - Eigen::Matrix3f R = svd.matrixU() * svd.matrixV().transpose(); - - double rx = R(2, 1) - R(1, 2); - double ry = R(0, 2) - R(2, 0); - double rz = R(1, 0) - R(0, 1); - - double s = sqrt((rx * rx + ry * ry + rz * rz) * 0.25); - double c = (R.trace() - 1) * 0.5; - c = c > 1. ? 1. : c < -1. ? -1. : c; - - double theta = acos(c); - - if (s < 1e-5) { - double t; - - if (c > 0) - rx = ry = rz = 0; - else { - t = (R(0, 0) + 1) * 0.5; - rx = sqrt(std::max(t, 0.0)); - t = (R(1, 1) + 1) * 0.5; - ry = sqrt(std::max(t, 0.0)) * (R(0, 1) < 0 ? -1.0 : 1.0); - t = (R(2, 2) + 1) * 0.5; - rz = sqrt(std::max(t, 0.0)) * (R(0, 2) < 0 ? -1.0 : 1.0); - - if (fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry * rz > 0)) - rz = -rz; - theta /= sqrt(rx * rx + ry * ry + rz * rz); - rx *= theta; - ry *= theta; - rz *= theta; - } - } else { - double vth = 1 / (2 * s); - vth *= theta; - rx *= vth; - ry *= vth; - rz *= vth; - } - return Eigen::Vector3d(rx, ry, rz).cast(); -} - // Sad times ahead IndexMap& ElasticFusion::getIndexMap() { return indexMap; @@ -931,8 +870,8 @@ const float& ElasticFusion::getMaxDepthProcessed() { return maxDepthProcessed; } -const Eigen::Matrix4f& ElasticFusion::getCurrPose() { - return currPose; +const Sophus::SE3d& ElasticFusion::get_T_wc() { + return T_wc_curr; } const int& ElasticFusion::getDeforms() { diff --git a/Core/ElasticFusion.h b/Core/ElasticFusion.h index 3c79d73f..cc35c88c 100644 --- a/Core/ElasticFusion.h +++ b/Core/ElasticFusion.h @@ -64,17 +64,15 @@ class ElasticFusion { * @param rgb uint8_t row major order * @param depth uint16_t z-depth in millimeters, invalid depths are 0 * @param timestamp nanoseconds (actually only used for the output poses, not important otherwise) - * @param inPose optional input SE3 pose (if provided, we don't attempt to perform tracking) * @param weightMultiplier optional full frame fusion weight - * @param bootstrap if true, use inPose as a pose guess rather than replacement + * @param T_wc optional input SE3 pose (if provided, we don't attempt to perform tracking) */ void processFrame( const uint8_t* rgb, const uint16_t* depth, const int64_t& timestamp, - const Eigen::Matrix4f* inPose = 0, - const float weightMultiplier = 1.f, - const bool bootstrap = false); + const float weightMultiplier, + const Sophus::SE3d* in_T_wc = 0); /** * Predicts the current view of the scene, updates the [vertex/normal/image]Tex() members @@ -218,7 +216,7 @@ class ElasticFusion { * The current global camera pose estimate * @return SE3 pose */ - const Eigen::Matrix4f& getCurrPose(); + const Sophus::SE3d& get_T_wc(); /** * The number of local deformations that have occurred @@ -283,9 +281,7 @@ class ElasticFusion { void processFerns(); - Eigen::Vector3f rodrigues2(const Eigen::Matrix3f& matrix); - - Eigen::Matrix4f currPose; + Sophus::SE3d T_wc_curr; int tick; const int timeDelta; @@ -301,8 +297,8 @@ class ElasticFusion { std::vector poseMatches; std::vector relativeCons; - std::vector> poseGraph; - std::vector poseLogTimes; + std::vector> t_T_wc; + std::vector poseLogTimes; Img> imageBuff; Img consBuff; diff --git a/Core/Ferns.cpp b/Core/Ferns.cpp index 0701116f..dbef1be0 100644 --- a/Core/Ferns.cpp +++ b/Core/Ferns.cpp @@ -79,7 +79,7 @@ bool Ferns::addFrame( GPUTexture* imageTexture, GPUTexture* vertexTexture, GPUTexture* normalTexture, - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, int srcTime, const float threshold) { Img> img(height, width); @@ -93,7 +93,7 @@ bool Ferns::addFrame( Frame* frame = new Frame( num, frames.size(), - pose, + T_wc, srcTime, width * height, (uint8_t*)img.data, @@ -159,9 +159,9 @@ bool Ferns::addFrame( } } -Eigen::Matrix4f Ferns::findFrame( +Sophus::SE3d Ferns::findFrame( std::vector& constraints, - const Eigen::Matrix4f& currPose, + const Sophus::SE3d& T_wc, GPUTexture* vertexTexture, GPUTexture* normalTexture, GPUTexture* imageTexture, @@ -177,7 +177,7 @@ Eigen::Matrix4f Ferns::findFrame( resize.vertex(vertexTexture, vertSmall); resize.vertex(normalTexture, normSmall); - Frame* frame = new Frame(num, 0, Eigen::Matrix4f::Identity(), 0, width * height); + Frame* frame = new Frame(num, 0, Sophus::SE3d(), 0, width * height); int* coOccurrences = new int[frames.size()]; @@ -223,10 +223,10 @@ Eigen::Matrix4f Ferns::findFrame( delete[] coOccurrences; - Eigen::Matrix4f estPose = Eigen::Matrix4f::Identity(); + Sophus::SE3d T_wc_est; if (minId != -1 && blockHDAware(frame, frames.at(minId)) > 0.3) { - Eigen::Matrix4f fernPose = frames.at(minId)->pose; + Sophus::SE3d T_wc_fern = frames.at(minId)->T_wc; vertFern.texture->Upload(frames.at(minId)->initVerts, GL_RGBA, GL_FLOAT); vertCurrent.texture->Upload(vertSmall.data, GL_RGBA, GL_FLOAT); @@ -238,24 +238,20 @@ Eigen::Matrix4f Ferns::findFrame( // colorCurrent.texture->Upload(imgSmall.data, GL_RGB, GL_UNSIGNED_BYTE); // WARNING initICP* must be called before initRGB* - rgbd.initICPModel(&vertFern, &normFern, fernPose); + rgbd.initICPModel(&vertFern, &normFern, T_wc_fern); // rgbd.initRGBModel(&colorFern); rgbd.initICP(&vertCurrent, &normCurrent); // rgbd.initRGB(&colorCurrent); - Eigen::Vector3f trans = fernPose.topRightCorner(3, 1); - Eigen::Matrix rot = fernPose.topLeftCorner(3, 3); + T_wc_est = T_wc_fern; TICK("fernOdom"); - rgbd.getIncrementalTransformation(trans, rot, false, 100, false, false, false); + rgbd.getIncrementalTransformation(T_wc_est, false, 100, false, false, false); TOCK("fernOdom"); - estPose.topRightCorner(3, 1) = trans; - estPose.topLeftCorner(3, 3) = rot; - float photoError = - photometricCheck(vertSmall, imgSmall, estPose, fernPose, frames.at(minId)->initRgb); + photometricCheck(vertSmall, imgSmall, T_wc_est, T_wc_fern, frames.at(minId)->initRgb); int icpCountThresh = lost ? 1400 : 2400; @@ -272,23 +268,23 @@ Eigen::Matrix4f Ferns::findFrame( int(vertSmall.at( conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) * 1000.0f) < maxDepth) { - Eigen::Vector4f worldRawPoint = currPose * - Eigen::Vector4f(vertSmall.at( + Eigen::Vector4d worldRawPoint = T_wc * + Eigen::Vector4d(vertSmall.at( conservatory.at(i).pos(1), conservatory.at(i).pos(0))(0), vertSmall.at( conservatory.at(i).pos(1), conservatory.at(i).pos(0))(1), vertSmall.at( conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2), - 1.0f); + 1.0); - Eigen::Vector4f worldModelPoint = estPose * - Eigen::Vector4f(vertSmall.at( + Eigen::Vector4d worldModelPoint = T_wc_est * + Eigen::Vector4d(vertSmall.at( conservatory.at(i).pos(1), conservatory.at(i).pos(0))(0), vertSmall.at( conservatory.at(i).pos(1), conservatory.at(i).pos(0))(1), vertSmall.at( conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2), - 1.0f); + 1.0); constraints.push_back(SurfaceConstraint(worldRawPoint, worldModelPoint)); } @@ -298,14 +294,14 @@ Eigen::Matrix4f Ferns::findFrame( delete frame; - return estPose; + return T_wc_est; } float Ferns::photometricCheck( const Img& vertSmall, const Img>& imgSmall, - const Eigen::Matrix4f& estPose, - const Eigen::Matrix4f& fernPose, + const Sophus::SE3d& T_wc_est, + const Sophus::SE3d& T_wc_fern, const uint8_t* fernRgb) { float cx = Intrinsics::getInstance().cx() / factor; float cy = Intrinsics::getInstance().cy() / factor; @@ -317,42 +313,48 @@ float Ferns::photometricCheck( float photoSum = 0; int photoCount = 0; + const Sophus::SE3f T_fern_est_f = (T_wc_fern.inverse() * T_wc_est).cast(); + for (int i = 0; i < num; i++) { if (vertSmall.at(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) > 0 && int(vertSmall.at(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) * 1000.0f) < maxDepth) { - Eigen::Vector4f vertPoint = Eigen::Vector4f( + Eigen::Vector4f vert_est = Eigen::Vector4f( vertSmall.at(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(0), vertSmall.at(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(1), vertSmall.at(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2), 1.0f); - Eigen::Matrix4f diff = fernPose.inverse() * estPose; - - Eigen::Vector4f worldCorrPoint = diff * vertPoint; - - Eigen::Vector2i correspondence( - (worldCorrPoint(0) * (1 / invfx) / worldCorrPoint(2) + cx), - (worldCorrPoint(1) * (1 / invfy) / worldCorrPoint(2) + cy)); - - if (correspondence(0) >= 0 && correspondence(1) >= 0 && correspondence(0) < width && - correspondence(1) < height && - (imgFern.at>(correspondence(1), correspondence(0))(0) > 0 || - imgFern.at>(correspondence(1), correspondence(0))(1) > 0 || - imgFern.at>(correspondence(1), correspondence(0))(2) > 0)) { - photoSum += abs( - (int)imgFern.at>(correspondence(1), correspondence(0))(0) - - (int)imgSmall.at>( - conservatory.at(i).pos(1), conservatory.at(i).pos(0))(0)); - photoSum += abs( - (int)imgFern.at>(correspondence(1), correspondence(0))(1) - - (int)imgSmall.at>( - conservatory.at(i).pos(1), conservatory.at(i).pos(0))(1)); - photoSum += abs( - (int)imgFern.at>(correspondence(1), correspondence(0))(2) - - (int)imgSmall.at>( - conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2)); + Eigen::Vector4f vert_fern = T_fern_est_f * vert_est; + + Eigen::Vector2i correspondence_px( + (vert_fern(0) * (1 / invfx) / vert_fern(2) + cx), + (vert_fern(1) * (1 / invfy) / vert_fern(2) + cy)); + + if (correspondence_px(0) >= 0 && correspondence_px(1) >= 0 && correspondence_px(0) < width && + correspondence_px(1) < height && + (imgFern.at>(correspondence_px(1), correspondence_px(0))(0) > + 0 || + imgFern.at>(correspondence_px(1), correspondence_px(0))(1) > + 0 || + imgFern.at>(correspondence_px(1), correspondence_px(0))(2) > + 0)) { + photoSum += + abs((int)imgFern.at>( + correspondence_px(1), correspondence_px(0))(0) - + (int)imgSmall.at>( + conservatory.at(i).pos(1), conservatory.at(i).pos(0))(0)); + photoSum += + abs((int)imgFern.at>( + correspondence_px(1), correspondence_px(0))(1) - + (int)imgSmall.at>( + conservatory.at(i).pos(1), conservatory.at(i).pos(0))(1)); + photoSum += + abs((int)imgFern.at>( + correspondence_px(1), correspondence_px(0))(2) - + (int)imgSmall.at>( + conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2)); photoCount++; } } diff --git a/Core/Ferns.h b/Core/Ferns.h index 32107e97..3c3ca246 100644 --- a/Core/Ferns.h +++ b/Core/Ferns.h @@ -40,22 +40,22 @@ class Ferns { GPUTexture* imageTexture, GPUTexture* vertexTexture, GPUTexture* normalTexture, - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, int srcTime, const float threshold); class SurfaceConstraint { public: - SurfaceConstraint(const Eigen::Vector4f& sourcePoint, const Eigen::Vector4f& targetPoint) + SurfaceConstraint(const Eigen::Vector4d& sourcePoint, const Eigen::Vector4d& targetPoint) : sourcePoint(sourcePoint), targetPoint(targetPoint) {} - Eigen::Vector4f sourcePoint; - Eigen::Vector4f targetPoint; + Eigen::Vector4d sourcePoint; + Eigen::Vector4d targetPoint; }; - Eigen::Matrix4f findFrame( + Sophus::SE3d findFrame( std::vector& constraints, - const Eigen::Matrix4f& currPose, + const Sophus::SE3d& T_wc, GPUTexture* vertexTexture, GPUTexture* normalTexture, GPUTexture* imageTexture, @@ -78,7 +78,7 @@ class Ferns { Frame( int n, int id, - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const int srcTime, const int numPixels, uint8_t* rgb = 0, @@ -86,7 +86,7 @@ class Ferns { Eigen::Vector4f* norms = 0) : goodCodes(0), id(id), - pose(pose), + T_wc(T_wc), srcTime(srcTime), initRgb(rgb), initVerts(verts), @@ -125,7 +125,7 @@ class Ferns { uint8_t* codes; int goodCodes; const int id; - Eigen::Matrix4f pose; + Sophus::SE3d T_wc; const int srcTime; uint8_t* initRgb; Eigen::Vector4f* initVerts; @@ -159,8 +159,8 @@ class Ferns { float photometricCheck( const Img& vertSmall, const Img>& imgSmall, - const Eigen::Matrix4f& estPose, - const Eigen::Matrix4f& fernPose, + const Sophus::SE3d& T_wc_est, + const Sophus::SE3d& T_wc_fern, const uint8_t* fernRgb); GPUTexture vertFern; diff --git a/Core/GlobalModel.cpp b/Core/GlobalModel.cpp index 57a91b88..01329f0d 100644 --- a/Core/GlobalModel.cpp +++ b/Core/GlobalModel.cpp @@ -354,7 +354,7 @@ const std::pair& GlobalModel::model() { } void GlobalModel::fuse( - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const int& time, GPUTexture* rgb, GPUTexture* depthRaw, @@ -364,7 +364,6 @@ void GlobalModel::fuse( GPUTexture* colorTimeMap, GPUTexture* normRadMap, const float depthCutoff, - const float confThreshold, const float weighting) { TICK("Fuse::Data"); // This first part does data association and computes the vertex to merge with, storing @@ -401,7 +400,7 @@ void GlobalModel::fuse( dataProgram->setUniform(Uniform("rows", (float)Resolution::getInstance().rows())); dataProgram->setUniform(Uniform("scale", (float)IndexMap::FACTOR)); dataProgram->setUniform(Uniform("texDim", (float)TEXTURE_DIMENSION)); - dataProgram->setUniform(Uniform("pose", pose)); + dataProgram->setUniform(Uniform("pose", T_wc.cast().matrix())); dataProgram->setUniform(Uniform("maxDepth", depthCutoff)); glEnableVertexAttribArray(0); @@ -526,7 +525,7 @@ void GlobalModel::fuse( } void GlobalModel::clean( - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const int& time, GPUTexture* indexMap, GPUTexture* vertConfMap, @@ -565,8 +564,8 @@ void GlobalModel::clean( unstableProgram->setUniform(Uniform("maxDepth", maxDepth)); unstableProgram->setUniform(Uniform("isFern", (int)isFern)); - Eigen::Matrix4f t_inv = pose.inverse(); - unstableProgram->setUniform(Uniform("t_inv", t_inv)); + const Eigen::Matrix4f T_cw = T_wc.inverse().matrix().cast(); + unstableProgram->setUniform(Uniform("t_inv", T_cw)); unstableProgram->setUniform(Uniform( "cam", diff --git a/Core/GlobalModel.h b/Core/GlobalModel.h index 4f4e8bf0..46e8a65d 100644 --- a/Core/GlobalModel.h +++ b/Core/GlobalModel.h @@ -58,7 +58,7 @@ class GlobalModel { const std::pair& model(); void fuse( - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const int& time, GPUTexture* rgb, GPUTexture* depthRaw, @@ -68,11 +68,10 @@ class GlobalModel { GPUTexture* colorTimeMap, GPUTexture* normRadMap, const float depthCutoff, - const float confThreshold, const float weighting); void clean( - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const int& time, GPUTexture* indexMap, GPUTexture* vertConfMap, diff --git a/Core/IndexMap.cpp b/Core/IndexMap.cpp index dea261d2..5d2e3b7b 100644 --- a/Core/IndexMap.cpp +++ b/Core/IndexMap.cpp @@ -188,7 +188,7 @@ IndexMap::IndexMap() IndexMap::~IndexMap() {} void IndexMap::predictIndices( - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const int& time, const std::pair& model, const float depthCutoff, @@ -205,7 +205,7 @@ void IndexMap::predictIndices( indexProgram->Bind(); - Eigen::Matrix4f t_inv = pose.inverse(); + const Eigen::Matrix4f T_cw = T_wc.inverse().matrix().cast(); Eigen::Vector4f cam( Intrinsics::getInstance().cx() * IndexMap::FACTOR, @@ -213,7 +213,7 @@ void IndexMap::predictIndices( Intrinsics::getInstance().fx() * IndexMap::FACTOR, Intrinsics::getInstance().fy() * IndexMap::FACTOR); - indexProgram->setUniform(Uniform("t_inv", t_inv)); + indexProgram->setUniform(Uniform("t_inv", T_cw)); indexProgram->setUniform(Uniform("cam", cam)); indexProgram->setUniform(Uniform("maxDepth", depthCutoff)); indexProgram->setUniform( @@ -291,7 +291,7 @@ void IndexMap::renderDepth(const float depthCutoff) { } void IndexMap::combinedPredict( - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const std::pair& model, const float depthCutoff, const float confThreshold, @@ -326,7 +326,7 @@ void IndexMap::combinedPredict( combinedProgram->Bind(); - Eigen::Matrix4f t_inv = pose.inverse(); + const Eigen::Matrix4f T_cw = T_wc.inverse().matrix().cast(); Eigen::Vector4f cam( Intrinsics::getInstance().cx(), @@ -334,7 +334,7 @@ void IndexMap::combinedPredict( Intrinsics::getInstance().fx(), Intrinsics::getInstance().fy()); - combinedProgram->setUniform(Uniform("t_inv", t_inv)); + combinedProgram->setUniform(Uniform("t_inv", T_cw)); combinedProgram->setUniform(Uniform("cam", cam)); combinedProgram->setUniform(Uniform("maxDepth", depthCutoff)); combinedProgram->setUniform(Uniform("confThreshold", confThreshold)); @@ -393,7 +393,7 @@ void IndexMap::combinedPredict( } void IndexMap::synthesizeDepth( - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const std::pair& model, const float depthCutoff, const float confThreshold, @@ -415,7 +415,7 @@ void IndexMap::synthesizeDepth( depthProgram->Bind(); - Eigen::Matrix4f t_inv = pose.inverse(); + const Eigen::Matrix4f T_cw = T_wc.inverse().matrix().cast(); Eigen::Vector4f cam( Intrinsics::getInstance().cx(), @@ -423,7 +423,7 @@ void IndexMap::synthesizeDepth( Intrinsics::getInstance().fx(), Intrinsics::getInstance().fy()); - depthProgram->setUniform(Uniform("t_inv", t_inv)); + depthProgram->setUniform(Uniform("t_inv", T_cw)); depthProgram->setUniform(Uniform("cam", cam)); depthProgram->setUniform(Uniform("maxDepth", depthCutoff)); depthProgram->setUniform(Uniform("confThreshold", confThreshold)); diff --git a/Core/IndexMap.h b/Core/IndexMap.h index 1b893369..0c80857d 100644 --- a/Core/IndexMap.h +++ b/Core/IndexMap.h @@ -21,6 +21,7 @@ #define INDEXMAP_H_ #include +#include #include #include "GPUTexture.h" #include "Shaders/Shaders.h" @@ -35,7 +36,7 @@ class IndexMap { virtual ~IndexMap(); void predictIndices( - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const int& time, const std::pair& model, const float depthCutoff, @@ -46,7 +47,7 @@ class IndexMap { enum Prediction { ACTIVE, INACTIVE }; void combinedPredict( - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const std::pair& model, const float depthCutoff, const float confThreshold, @@ -62,7 +63,7 @@ class IndexMap { const float confThreshold); void synthesizeDepth( - const Eigen::Matrix4f& pose, + const Sophus::SE3d& T_wc, const std::pair& model, const float depthCutoff, const float confThreshold, diff --git a/Core/PoseMatch.h b/Core/PoseMatch.h index 09b65a89..c2705d8a 100644 --- a/Core/PoseMatch.h +++ b/Core/PoseMatch.h @@ -28,21 +28,21 @@ class PoseMatch { PoseMatch( int firstId, int secondId, - const Eigen::Matrix4f& first, - const Eigen::Matrix4f& second, + const Sophus::SE3d& T_wc_first, + const Sophus::SE3d& T_wc_second, const std::vector& constraints, const bool& fern) : firstId(firstId), secondId(secondId), - first(first), - second(second), + T_wc_first(T_wc_first), + T_wc_second(T_wc_second), constraints(constraints), fern(fern) {} int firstId; int secondId; - Eigen::Matrix4f first; - Eigen::Matrix4f second; + Sophus::SE3d T_wc_first; + Sophus::SE3d T_wc_second; std::vector constraints; bool fern; }; diff --git a/Core/Utils/DeformationGraph.cpp b/Core/Utils/DeformationGraph.cpp index 5996fff0..563758af 100644 --- a/Core/Utils/DeformationGraph.cpp +++ b/Core/Utils/DeformationGraph.cpp @@ -20,14 +20,14 @@ #include "DeformationGraph.h" #include "CholeskyDecomp.h" -DeformationGraph::DeformationGraph(int k, std::vector* sourceVertices) +DeformationGraph::DeformationGraph(int k, std::vector* sourceVertices) : k(k), initialised(false), wRot(1), wReg(10), wCon(100), sourceVertices(sourceVertices), - graphCloud(new std::vector), + graphCloud(new std::vector), lastPointCount(0), cholesky(new CholeskyDecomp) {} @@ -45,13 +45,13 @@ std::vector& DeformationGraph::getGraph() { return graph; } -std::vector& DeformationGraph::getGraphTimes() { +std::vector& DeformationGraph::getGraphTimes() { return sampledGraphTimes; } void DeformationGraph::initialiseGraph( - std::vector* customGraph, - std::vector* graphTimeMap) { + std::vector* customGraph, + std::vector* graphTimeMap) { graphCloud->clear(); sampledGraphTimes.clear(); @@ -73,7 +73,7 @@ void DeformationGraph::initialiseGraph( graphNodes[i].position = graphCloud->at(i); - graphNodes[i].translation = Eigen::Vector3f::Zero(); + graphNodes[i].translation = Eigen::Vector3d::Zero(); graphNodes[i].rotation.setIdentity(); @@ -86,7 +86,7 @@ void DeformationGraph::initialiseGraph( } void DeformationGraph::appendVertices( - std::vector* vertexTimeMap, + std::vector* vertexTimeMap, uint32_t originalPointEnd) { vertexMap.resize(lastPointCount); @@ -95,48 +95,45 @@ void DeformationGraph::appendVertices( lastPointCount = originalPointEnd; } -void DeformationGraph::applyGraphToPoses(std::vector& poses) { - assert(poses.size() == poseMap.size() && initialised); +void DeformationGraph::applyGraphToPoses(std::vector T_wc_ptrs) { + assert(T_wc_ptrs.size() == poseMap.size() && initialised); - Eigen::Vector3f newPosition; - Eigen::Matrix3f rotation; + Eigen::Vector3d newPosition; + Eigen::Matrix3d rotation; - for (size_t i = 0; i < poses.size(); i++) { + for (size_t i = 0; i < T_wc_ptrs.size(); i++) { std::vector& weightMap = poseMap.at(i); - newPosition = Eigen::Vector3f::Zero(); - rotation = Eigen::Matrix3f::Zero(); + newPosition = Eigen::Vector3d::Zero(); + rotation = Eigen::Matrix3d::Zero(); for (size_t j = 0; j < weightMap.size(); j++) { newPosition += weightMap.at(j).weight * (graph.at(weightMap.at(j).node)->rotation * - (poses.at(i)->topRightCorner(3, 1) - graph.at(weightMap.at(j).node)->position) + + (T_wc_ptrs.at(i)->translation() - graph.at(weightMap.at(j).node)->position) + graph.at(weightMap.at(j).node)->position + graph.at(weightMap.at(j).node)->translation); rotation += weightMap.at(j).weight * graph.at(weightMap.at(j).node)->rotation; } - Eigen::Matrix3f newRotation = rotation * poses.at(i)->topLeftCorner(3, 3); + Eigen::Matrix3d newRotation = rotation * T_wc_ptrs.at(i)->rotationMatrix(); - Eigen::JacobiSVD svd(newRotation, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::JacobiSVD svd(newRotation, Eigen::ComputeFullU | Eigen::ComputeFullV); - poses.at(i)->topRightCorner(3, 1) = newPosition; - poses.at(i)->topLeftCorner(3, 3) = svd.matrixU() * svd.matrixV().transpose(); + T_wc_ptrs.at(i)->translation() = newPosition; + T_wc_ptrs.at(i)->rotationMatrix() = svd.matrixU() * svd.matrixV().transpose(); } } void DeformationGraph::setPosesSeq( - std::vector* poseTimeMap, - const std::vector& poses) { + std::vector* poseTimeMap, + const std::vector& T_wcs) { poseMap.clear(); const uint32_t lookBack = 20; - std::vector pointIdxNKNSearch(k + 1); - std::vector pointNKNSquaredDistance(k + 1); - - for (uint32_t i = 0; i < poses.size(); i++) { - unsigned long long int poseTime = poseTimeMap->at(i); + for (uint32_t i = 0; i < T_wcs.size(); i++) { + uint64_t poseTime = poseTimeMap->at(i); uint32_t foundIndex = 0; @@ -182,7 +179,7 @@ void DeformationGraph::setPosesSeq( uint32_t distanceBack = 0; for (int j = (int)foundIndex; j >= 0; j--) { std::pair newNode; - newNode.first = (graphCloud->at(j) - poses.at(i).topRightCorner(3, 1)).norm(); + newNode.first = (graphCloud->at(j) - T_wcs.at(i).translation()).norm(); newNode.second = j; nearNodes.push_back(newNode); @@ -195,7 +192,7 @@ void DeformationGraph::setPosesSeq( if (distanceBack != lookBack) { for (uint32_t j = foundIndex + 1; j < sampledGraphTimes.size(); j++) { std::pair newNode; - newNode.first = (graphCloud->at(j) - poses.at(i).topRightCorner(3, 1)).norm(); + newNode.first = (graphCloud->at(j) - T_wcs.at(i).translation()).norm(); newNode.second = j; nearNodes.push_back(newNode); @@ -213,7 +210,7 @@ void DeformationGraph::setPosesSeq( return left.first < right.first; }); - Eigen::Vector3f vertexPosition = poses.at(i).topRightCorner(3, 1); + Eigen::Vector3d vertexPosition = T_wcs.at(i).translation(); double dMax = nearNodes.at(k).first; std::vector newMap; @@ -222,8 +219,9 @@ void DeformationGraph::setPosesSeq( for (uint32_t j = 0; j < (uint32_t)k; j++) { newMap.push_back(VertexWeightMap( - pow(1.0f - (vertexPosition - graphNodes[nearNodes.at(j).second].position).norm() / dMax, - 2), + std::pow( + 1.0 - (vertexPosition - graphNodes[nearNodes.at(j).second].position).norm() / dMax, + 2.0), nearNodes.at(j).second)); weightSum += newMap.back().weight; } @@ -267,14 +265,11 @@ void DeformationGraph::connectGraphSeq() { } } -void DeformationGraph::weightVerticesSeq(std::vector* vertexTimeMap) { +void DeformationGraph::weightVerticesSeq(std::vector* vertexTimeMap) { const uint32_t lookBack = 20; - std::vector pointIdxNKNSearch(k + 1); - std::vector pointNKNSquaredDistance(k + 1); - for (uint32_t i = lastPointCount; i < sourceVertices->size(); i++) { - unsigned long long int vertexTime = vertexTimeMap->at(i); + uint64_t vertexTime = vertexTimeMap->at(i); uint32_t foundIndex = 0; @@ -351,7 +346,7 @@ void DeformationGraph::weightVerticesSeq(std::vector* ve return left.first < right.first; }); - Eigen::Vector3f vertexPosition = sourceVertices->at(i); + Eigen::Vector3d vertexPosition = sourceVertices->at(i); double dMax = nearNodes.at(k).first; std::vector newMap; @@ -360,8 +355,9 @@ void DeformationGraph::weightVerticesSeq(std::vector* ve for (uint32_t j = 0; j < (uint32_t)k; j++) { newMap.push_back(VertexWeightMap( - pow(1.0f - (vertexPosition - graphNodes[nearNodes.at(j).second].position).norm() / dMax, - 2), + std::pow( + 1.0 - (vertexPosition - graphNodes[nearNodes.at(j).second].position).norm() / dMax, + 2.0), nearNodes.at(j).second)); weightSum += newMap.back().weight; } @@ -377,7 +373,7 @@ void DeformationGraph::weightVerticesSeq(std::vector* ve } void DeformationGraph::applyGraphToVertices() { - Eigen::Vector3f position; + Eigen::Vector3d position; for (uint32_t i = 0; i < sourceVertices->size(); i++) { computeVertexPosition(i, position); @@ -385,7 +381,7 @@ void DeformationGraph::applyGraphToVertices() { } } -void DeformationGraph::addConstraint(int vertexId, Eigen::Vector3f& target) { +void DeformationGraph::addConstraint(int vertexId, Eigen::Vector3d& target) { assert(initialised); // Overwrites old constraint @@ -421,7 +417,7 @@ bool DeformationGraph::optimiseGraphSparse( float& error, float& meanConsErr, const bool fernMatch, - const unsigned long long int lastDeformTime) { + const uint64_t lastDeformTime) { assert(initialised); TICK("opt"); @@ -510,7 +506,7 @@ void DeformationGraph::sparseJacobian( int colOffset = graph.at(j)->id * numVariables; // No weights for rotation as rotation weight = 1 - const Eigen::Matrix3f& rotation = graph.at(j)->rotation; + const Eigen::Matrix3d& rotation = graph.at(j)->rotation; rows[lastRow] = new OrderedJacobianRow(6); rows[lastRow + 1] = new OrderedJacobianRow(6); @@ -566,7 +562,7 @@ void DeformationGraph::sparseJacobian( rows[lastRow + 1] = new OrderedJacobianRow(5); rows[lastRow + 2] = new OrderedJacobianRow(5); - Eigen::Vector3f delta = + Eigen::Vector3d delta = graph.at(graph.at(j)->neighbours.at(n))->position - graph.at(j)->position; int colOffsetN = graph.at(graph.at(j)->neighbours.at(n))->id * numVariables; @@ -574,32 +570,32 @@ void DeformationGraph::sparseJacobian( assert(colOffset != colOffsetN); if (colOffsetN < colOffset && graph.at(graph.at(j)->neighbours.at(n))->enabled) { - rows[lastRow]->append(colOffsetN + 9 - backSet, -1.0 * sqrt(wReg)); - rows[lastRow + 1]->append(colOffsetN + 10 - backSet, -1.0 * sqrt(wReg)); - rows[lastRow + 2]->append(colOffsetN + 11 - backSet, -1.0 * sqrt(wReg)); + rows[lastRow]->append(colOffsetN + 9 - backSet, -1.0 * std::sqrt(wReg)); + rows[lastRow + 1]->append(colOffsetN + 10 - backSet, -1.0 * std::sqrt(wReg)); + rows[lastRow + 2]->append(colOffsetN + 11 - backSet, -1.0 * std::sqrt(wReg)); } if (graph.at(j)->enabled) { - rows[lastRow]->append(colOffset - backSet, delta(0) * sqrt(wReg)); - rows[lastRow]->append(colOffset + 3 - backSet, delta(1) * sqrt(wReg)); - rows[lastRow]->append(colOffset + 6 - backSet, delta(2) * sqrt(wReg)); - rows[lastRow]->append(colOffset + 9 - backSet, 1.0 * sqrt(wReg)); - - rows[lastRow + 1]->append(colOffset + 1 - backSet, delta(0) * sqrt(wReg)); - rows[lastRow + 1]->append(colOffset + 4 - backSet, delta(1) * sqrt(wReg)); - rows[lastRow + 1]->append(colOffset + 7 - backSet, delta(2) * sqrt(wReg)); - rows[lastRow + 1]->append(colOffset + 10 - backSet, 1.0 * sqrt(wReg)); - - rows[lastRow + 2]->append(colOffset + 2 - backSet, delta(0) * sqrt(wReg)); - rows[lastRow + 2]->append(colOffset + 5 - backSet, delta(1) * sqrt(wReg)); - rows[lastRow + 2]->append(colOffset + 8 - backSet, delta(2) * sqrt(wReg)); - rows[lastRow + 2]->append(colOffset + 11 - backSet, 1.0 * sqrt(wReg)); + rows[lastRow]->append(colOffset - backSet, delta(0) * std::sqrt(wReg)); + rows[lastRow]->append(colOffset + 3 - backSet, delta(1) * std::sqrt(wReg)); + rows[lastRow]->append(colOffset + 6 - backSet, delta(2) * std::sqrt(wReg)); + rows[lastRow]->append(colOffset + 9 - backSet, 1.0 * std::sqrt(wReg)); + + rows[lastRow + 1]->append(colOffset + 1 - backSet, delta(0) * std::sqrt(wReg)); + rows[lastRow + 1]->append(colOffset + 4 - backSet, delta(1) * std::sqrt(wReg)); + rows[lastRow + 1]->append(colOffset + 7 - backSet, delta(2) * std::sqrt(wReg)); + rows[lastRow + 1]->append(colOffset + 10 - backSet, 1.0 * std::sqrt(wReg)); + + rows[lastRow + 2]->append(colOffset + 2 - backSet, delta(0) * std::sqrt(wReg)); + rows[lastRow + 2]->append(colOffset + 5 - backSet, delta(1) * std::sqrt(wReg)); + rows[lastRow + 2]->append(colOffset + 8 - backSet, delta(2) * std::sqrt(wReg)); + rows[lastRow + 2]->append(colOffset + 11 - backSet, 1.0 * std::sqrt(wReg)); } if (colOffsetN > colOffset && graph.at(graph.at(j)->neighbours.at(n))->enabled) { - rows[lastRow]->append(colOffsetN + 9 - backSet, -1.0 * sqrt(wReg)); - rows[lastRow + 1]->append(colOffsetN + 10 - backSet, -1.0 * sqrt(wReg)); - rows[lastRow + 2]->append(colOffsetN + 11 - backSet, -1.0 * sqrt(wReg)); + rows[lastRow]->append(colOffsetN + 9 - backSet, -1.0 * std::sqrt(wReg)); + rows[lastRow + 1]->append(colOffsetN + 10 - backSet, -1.0 * std::sqrt(wReg)); + rows[lastRow + 2]->append(colOffsetN + 11 - backSet, -1.0 * std::sqrt(wReg)); } lastRow += eRegRows; @@ -631,7 +627,7 @@ void DeformationGraph::sparseJacobian( } if (nodeInfluences) { - Eigen::Vector3f sourcePosition = sourceVertices->at(constraints.at(l).vertexId); + Eigen::Vector3d sourcePosition = sourceVertices->at(constraints.at(l).vertexId); rows[lastRow] = new OrderedJacobianRow(4 * k * 2); rows[lastRow + 1] = new OrderedJacobianRow(4 * k * 2); @@ -640,7 +636,7 @@ void DeformationGraph::sparseJacobian( assert(graph.at(weightMap.at(0).node)->id < graph.at(weightMap.at(1).node)->id); if (constraints.at(l).relative) { - Eigen::Vector3f targetPosition = sourceVertices->at(constraints.at(l).targetId); + Eigen::Vector3d targetPosition = sourceVertices->at(constraints.at(l).targetId); std::vector& relWeightMap = vertexMap.at(constraints.at(l).targetId); @@ -662,90 +658,90 @@ void DeformationGraph::sparseJacobian( int colOffset = graph.at(weightMapMixed.at(i).node)->id * numVariables; if (weightMapMixed.at(i).relative) { - Eigen::Vector3f delta = + Eigen::Vector3d delta = (graph.at(weightMapMixed.at(i).node)->position - targetPosition) * weightMapMixed.at(i).weight; // We have to sum the Jacobian entries in this case if (checkList[graph.at(weightMapMixed.at(i).node)->id]) { - rows[lastRow]->addTo(colOffset - backSet, delta(0), sqrt(wCon)); - rows[lastRow]->addTo(colOffset + 3 - backSet, delta(1), sqrt(wCon)); - rows[lastRow]->addTo(colOffset + 6 - backSet, delta(2), sqrt(wCon)); + rows[lastRow]->addTo(colOffset - backSet, delta(0), std::sqrt(wCon)); + rows[lastRow]->addTo(colOffset + 3 - backSet, delta(1), std::sqrt(wCon)); + rows[lastRow]->addTo(colOffset + 6 - backSet, delta(2), std::sqrt(wCon)); rows[lastRow]->addTo( - colOffset + 9 - backSet, -weightMapMixed.at(i).weight, sqrt(wCon)); + colOffset + 9 - backSet, -weightMapMixed.at(i).weight, std::sqrt(wCon)); - rows[lastRow + 1]->addTo(colOffset + 1 - backSet, delta(0), sqrt(wCon)); - rows[lastRow + 1]->addTo(colOffset + 4 - backSet, delta(1), sqrt(wCon)); - rows[lastRow + 1]->addTo(colOffset + 7 - backSet, delta(2), sqrt(wCon)); + rows[lastRow + 1]->addTo(colOffset + 1 - backSet, delta(0), std::sqrt(wCon)); + rows[lastRow + 1]->addTo(colOffset + 4 - backSet, delta(1), std::sqrt(wCon)); + rows[lastRow + 1]->addTo(colOffset + 7 - backSet, delta(2), std::sqrt(wCon)); rows[lastRow + 1]->addTo( - colOffset + 10 - backSet, -weightMapMixed.at(i).weight, sqrt(wCon)); + colOffset + 10 - backSet, -weightMapMixed.at(i).weight, std::sqrt(wCon)); - rows[lastRow + 2]->addTo(colOffset + 2 - backSet, delta(0), sqrt(wCon)); - rows[lastRow + 2]->addTo(colOffset + 5 - backSet, delta(1), sqrt(wCon)); - rows[lastRow + 2]->addTo(colOffset + 8 - backSet, delta(2), sqrt(wCon)); + rows[lastRow + 2]->addTo(colOffset + 2 - backSet, delta(0), std::sqrt(wCon)); + rows[lastRow + 2]->addTo(colOffset + 5 - backSet, delta(1), std::sqrt(wCon)); + rows[lastRow + 2]->addTo(colOffset + 8 - backSet, delta(2), std::sqrt(wCon)); rows[lastRow + 2]->addTo( - colOffset + 11 - backSet, -weightMapMixed.at(i).weight, sqrt(wCon)); + colOffset + 11 - backSet, -weightMapMixed.at(i).weight, std::sqrt(wCon)); } else { - rows[lastRow]->append(colOffset - backSet, delta(0) * sqrt(wCon)); - rows[lastRow]->append(colOffset + 3 - backSet, delta(1) * sqrt(wCon)); - rows[lastRow]->append(colOffset + 6 - backSet, delta(2) * sqrt(wCon)); + rows[lastRow]->append(colOffset - backSet, delta(0) * std::sqrt(wCon)); + rows[lastRow]->append(colOffset + 3 - backSet, delta(1) * std::sqrt(wCon)); + rows[lastRow]->append(colOffset + 6 - backSet, delta(2) * std::sqrt(wCon)); rows[lastRow]->append( - colOffset + 9 - backSet, -weightMapMixed.at(i).weight * sqrt(wCon)); + colOffset + 9 - backSet, -weightMapMixed.at(i).weight * std::sqrt(wCon)); - rows[lastRow + 1]->append(colOffset + 1 - backSet, delta(0) * sqrt(wCon)); - rows[lastRow + 1]->append(colOffset + 4 - backSet, delta(1) * sqrt(wCon)); - rows[lastRow + 1]->append(colOffset + 7 - backSet, delta(2) * sqrt(wCon)); + rows[lastRow + 1]->append(colOffset + 1 - backSet, delta(0) * std::sqrt(wCon)); + rows[lastRow + 1]->append(colOffset + 4 - backSet, delta(1) * std::sqrt(wCon)); + rows[lastRow + 1]->append(colOffset + 7 - backSet, delta(2) * std::sqrt(wCon)); rows[lastRow + 1]->append( - colOffset + 10 - backSet, -weightMapMixed.at(i).weight * sqrt(wCon)); + colOffset + 10 - backSet, -weightMapMixed.at(i).weight * std::sqrt(wCon)); - rows[lastRow + 2]->append(colOffset + 2 - backSet, delta(0) * sqrt(wCon)); - rows[lastRow + 2]->append(colOffset + 5 - backSet, delta(1) * sqrt(wCon)); - rows[lastRow + 2]->append(colOffset + 8 - backSet, delta(2) * sqrt(wCon)); + rows[lastRow + 2]->append(colOffset + 2 - backSet, delta(0) * std::sqrt(wCon)); + rows[lastRow + 2]->append(colOffset + 5 - backSet, delta(1) * std::sqrt(wCon)); + rows[lastRow + 2]->append(colOffset + 8 - backSet, delta(2) * std::sqrt(wCon)); rows[lastRow + 2]->append( - colOffset + 11 - backSet, -weightMapMixed.at(i).weight * sqrt(wCon)); + colOffset + 11 - backSet, -weightMapMixed.at(i).weight * std::sqrt(wCon)); } } else { - Eigen::Vector3f delta = + Eigen::Vector3d delta = (sourcePosition - graph.at(weightMapMixed.at(i).node)->position) * weightMapMixed.at(i).weight; // We have to sum the Jacobian entries in this case if (checkList[graph.at(weightMapMixed.at(i).node)->id]) { - rows[lastRow]->addTo(colOffset - backSet, delta(0), sqrt(wCon)); - rows[lastRow]->addTo(colOffset + 3 - backSet, delta(1), sqrt(wCon)); - rows[lastRow]->addTo(colOffset + 6 - backSet, delta(2), sqrt(wCon)); + rows[lastRow]->addTo(colOffset - backSet, delta(0), std::sqrt(wCon)); + rows[lastRow]->addTo(colOffset + 3 - backSet, delta(1), std::sqrt(wCon)); + rows[lastRow]->addTo(colOffset + 6 - backSet, delta(2), std::sqrt(wCon)); rows[lastRow]->addTo( - colOffset + 9 - backSet, weightMapMixed.at(i).weight, sqrt(wCon)); + colOffset + 9 - backSet, weightMapMixed.at(i).weight, std::sqrt(wCon)); - rows[lastRow + 1]->addTo(colOffset + 1 - backSet, delta(0), sqrt(wCon)); - rows[lastRow + 1]->addTo(colOffset + 4 - backSet, delta(1), sqrt(wCon)); - rows[lastRow + 1]->addTo(colOffset + 7 - backSet, delta(2), sqrt(wCon)); + rows[lastRow + 1]->addTo(colOffset + 1 - backSet, delta(0), std::sqrt(wCon)); + rows[lastRow + 1]->addTo(colOffset + 4 - backSet, delta(1), std::sqrt(wCon)); + rows[lastRow + 1]->addTo(colOffset + 7 - backSet, delta(2), std::sqrt(wCon)); rows[lastRow + 1]->addTo( - colOffset + 10 - backSet, weightMapMixed.at(i).weight, sqrt(wCon)); + colOffset + 10 - backSet, weightMapMixed.at(i).weight, std::sqrt(wCon)); - rows[lastRow + 2]->addTo(colOffset + 2 - backSet, delta(0), sqrt(wCon)); - rows[lastRow + 2]->addTo(colOffset + 5 - backSet, delta(1), sqrt(wCon)); - rows[lastRow + 2]->addTo(colOffset + 8 - backSet, delta(2), sqrt(wCon)); + rows[lastRow + 2]->addTo(colOffset + 2 - backSet, delta(0), std::sqrt(wCon)); + rows[lastRow + 2]->addTo(colOffset + 5 - backSet, delta(1), std::sqrt(wCon)); + rows[lastRow + 2]->addTo(colOffset + 8 - backSet, delta(2), std::sqrt(wCon)); rows[lastRow + 2]->addTo( - colOffset + 11 - backSet, weightMapMixed.at(i).weight, sqrt(wCon)); + colOffset + 11 - backSet, weightMapMixed.at(i).weight, std::sqrt(wCon)); } else { - rows[lastRow]->append(colOffset - backSet, delta(0) * sqrt(wCon)); - rows[lastRow]->append(colOffset + 3 - backSet, delta(1) * sqrt(wCon)); - rows[lastRow]->append(colOffset + 6 - backSet, delta(2) * sqrt(wCon)); + rows[lastRow]->append(colOffset - backSet, delta(0) * std::sqrt(wCon)); + rows[lastRow]->append(colOffset + 3 - backSet, delta(1) * std::sqrt(wCon)); + rows[lastRow]->append(colOffset + 6 - backSet, delta(2) * std::sqrt(wCon)); rows[lastRow]->append( - colOffset + 9 - backSet, weightMapMixed.at(i).weight * sqrt(wCon)); + colOffset + 9 - backSet, weightMapMixed.at(i).weight * std::sqrt(wCon)); - rows[lastRow + 1]->append(colOffset + 1 - backSet, delta(0) * sqrt(wCon)); - rows[lastRow + 1]->append(colOffset + 4 - backSet, delta(1) * sqrt(wCon)); - rows[lastRow + 1]->append(colOffset + 7 - backSet, delta(2) * sqrt(wCon)); + rows[lastRow + 1]->append(colOffset + 1 - backSet, delta(0) * std::sqrt(wCon)); + rows[lastRow + 1]->append(colOffset + 4 - backSet, delta(1) * std::sqrt(wCon)); + rows[lastRow + 1]->append(colOffset + 7 - backSet, delta(2) * std::sqrt(wCon)); rows[lastRow + 1]->append( - colOffset + 10 - backSet, weightMapMixed.at(i).weight * sqrt(wCon)); + colOffset + 10 - backSet, weightMapMixed.at(i).weight * std::sqrt(wCon)); - rows[lastRow + 2]->append(colOffset + 2 - backSet, delta(0) * sqrt(wCon)); - rows[lastRow + 2]->append(colOffset + 5 - backSet, delta(1) * sqrt(wCon)); - rows[lastRow + 2]->append(colOffset + 8 - backSet, delta(2) * sqrt(wCon)); + rows[lastRow + 2]->append(colOffset + 2 - backSet, delta(0) * std::sqrt(wCon)); + rows[lastRow + 2]->append(colOffset + 5 - backSet, delta(1) * std::sqrt(wCon)); + rows[lastRow + 2]->append(colOffset + 8 - backSet, delta(2) * std::sqrt(wCon)); rows[lastRow + 2]->append( - colOffset + 11 - backSet, weightMapMixed.at(i).weight * sqrt(wCon)); + colOffset + 11 - backSet, weightMapMixed.at(i).weight * std::sqrt(wCon)); } } @@ -759,25 +755,26 @@ void DeformationGraph::sparseJacobian( if (graph.at(weightMap.at(i).node)->enabled) { int colOffset = graph.at(weightMap.at(i).node)->id * numVariables; - Eigen::Vector3f delta = (sourcePosition - graph.at(weightMap.at(i).node)->position) * + Eigen::Vector3d delta = (sourcePosition - graph.at(weightMap.at(i).node)->position) * weightMap.at(i).weight; - rows[lastRow]->append(colOffset - backSet, delta(0) * sqrt(wCon)); - rows[lastRow]->append(colOffset + 3 - backSet, delta(1) * sqrt(wCon)); - rows[lastRow]->append(colOffset + 6 - backSet, delta(2) * sqrt(wCon)); - rows[lastRow]->append(colOffset + 9 - backSet, weightMap.at(i).weight * sqrt(wCon)); + rows[lastRow]->append(colOffset - backSet, delta(0) * std::sqrt(wCon)); + rows[lastRow]->append(colOffset + 3 - backSet, delta(1) * std::sqrt(wCon)); + rows[lastRow]->append(colOffset + 6 - backSet, delta(2) * std::sqrt(wCon)); + rows[lastRow]->append( + colOffset + 9 - backSet, weightMap.at(i).weight * std::sqrt(wCon)); - rows[lastRow + 1]->append(colOffset + 1 - backSet, delta(0) * sqrt(wCon)); - rows[lastRow + 1]->append(colOffset + 4 - backSet, delta(1) * sqrt(wCon)); - rows[lastRow + 1]->append(colOffset + 7 - backSet, delta(2) * sqrt(wCon)); + rows[lastRow + 1]->append(colOffset + 1 - backSet, delta(0) * std::sqrt(wCon)); + rows[lastRow + 1]->append(colOffset + 4 - backSet, delta(1) * std::sqrt(wCon)); + rows[lastRow + 1]->append(colOffset + 7 - backSet, delta(2) * std::sqrt(wCon)); rows[lastRow + 1]->append( - colOffset + 10 - backSet, weightMap.at(i).weight * sqrt(wCon)); + colOffset + 10 - backSet, weightMap.at(i).weight * std::sqrt(wCon)); - rows[lastRow + 2]->append(colOffset + 2 - backSet, delta(0) * sqrt(wCon)); - rows[lastRow + 2]->append(colOffset + 5 - backSet, delta(1) * sqrt(wCon)); - rows[lastRow + 2]->append(colOffset + 8 - backSet, delta(2) * sqrt(wCon)); + rows[lastRow + 2]->append(colOffset + 2 - backSet, delta(0) * std::sqrt(wCon)); + rows[lastRow + 2]->append(colOffset + 5 - backSet, delta(1) * std::sqrt(wCon)); + rows[lastRow + 2]->append(colOffset + 8 - backSet, delta(2) * std::sqrt(wCon)); rows[lastRow + 2]->append( - colOffset + 11 - backSet, weightMap.at(i).weight * sqrt(wCon)); + colOffset + 11 - backSet, weightMap.at(i).weight * std::sqrt(wCon)); } } } @@ -800,7 +797,7 @@ Eigen::VectorXd DeformationGraph::sparseResidual(const int maxRows) { for (uint32_t j = 0; j < graph.size(); j++) { if (graph.at(j)->enabled) { // No weights for rotation as rotation weight = 1 - const Eigen::Matrix3f& rotation = graph.at(j)->rotation; + const Eigen::Matrix3d& rotation = graph.at(j)->rotation; // ab + de + gh residual(numRows) = rotation.col(0).dot(rotation.col(1)); @@ -832,9 +829,8 @@ Eigen::VectorXd DeformationGraph::sparseResidual(const int maxRows) { (graph.at(graph.at(j)->neighbours.at(n))->position - graph.at(j)->position) + graph.at(j)->position + graph.at(j)->translation - (graph.at(graph.at(j)->neighbours.at(n))->position + - graph.at(graph.at(j)->neighbours.at(n))->translation)) - .cast() * - sqrt(wReg); + graph.at(graph.at(j)->neighbours.at(n))->translation)) * + std::sqrt(wReg); numRows += eRegRows; } } @@ -865,20 +861,20 @@ Eigen::VectorXd DeformationGraph::sparseResidual(const int maxRows) { if (nodeInfluences) { if (constraints.at(l).relative) { - Eigen::Vector3f srcPos, tarPos; + Eigen::Vector3d srcPos, tarPos; computeVertexPosition(constraints.at(l).vertexId, srcPos); computeVertexPosition(constraints.at(l).targetId, tarPos); - residual.segment(numRows, 3) = (srcPos - tarPos).cast() * sqrt(wCon); + residual.segment(numRows, 3) = (srcPos - tarPos) * std::sqrt(wCon); } else { - Eigen::Vector3f position; + Eigen::Vector3d position; computeVertexPosition(constraints.at(l).vertexId, position); residual.segment(numRows, 3) = - (position - constraints.at(l).targetPosition).cast() * sqrt(wCon); + (position - constraints.at(l).targetPosition) * std::sqrt(wCon); } numRows += eConRows; @@ -905,28 +901,28 @@ void DeformationGraph::applyDeltaSparse(Eigen::VectorXd& delta) { for (uint32_t j = 0; j < graph.size(); j++) { if (graph.at(j)->enabled) { - const_cast(graph.at(j)->rotation.data())[0] += delta(z + 0); - const_cast(graph.at(j)->rotation.data())[1] += delta(z + 1); - const_cast(graph.at(j)->rotation.data())[2] += delta(z + 2); + const_cast(graph.at(j)->rotation.data())[0] += delta(z + 0); + const_cast(graph.at(j)->rotation.data())[1] += delta(z + 1); + const_cast(graph.at(j)->rotation.data())[2] += delta(z + 2); - const_cast(graph.at(j)->rotation.data())[3] += delta(z + 3); - const_cast(graph.at(j)->rotation.data())[4] += delta(z + 4); - const_cast(graph.at(j)->rotation.data())[5] += delta(z + 5); + const_cast(graph.at(j)->rotation.data())[3] += delta(z + 3); + const_cast(graph.at(j)->rotation.data())[4] += delta(z + 4); + const_cast(graph.at(j)->rotation.data())[5] += delta(z + 5); - const_cast(graph.at(j)->rotation.data())[6] += delta(z + 6); - const_cast(graph.at(j)->rotation.data())[7] += delta(z + 7); - const_cast(graph.at(j)->rotation.data())[8] += delta(z + 8); + const_cast(graph.at(j)->rotation.data())[6] += delta(z + 6); + const_cast(graph.at(j)->rotation.data())[7] += delta(z + 7); + const_cast(graph.at(j)->rotation.data())[8] += delta(z + 8); - const_cast(graph.at(j)->translation.data())[0] += delta(z + 9); - const_cast(graph.at(j)->translation.data())[1] += delta(z + 10); - const_cast(graph.at(j)->translation.data())[2] += delta(z + 11); + const_cast(graph.at(j)->translation.data())[0] += delta(z + 9); + const_cast(graph.at(j)->translation.data())[1] += delta(z + 10); + const_cast(graph.at(j)->translation.data())[2] += delta(z + 11); z += numVariables; } } } -void DeformationGraph::computeVertexPosition(int vertexId, Eigen::Vector3f& position) { +void DeformationGraph::computeVertexPosition(int vertexId, Eigen::Vector3d& position) { assert(initialised); std::vector& weightMap = vertexMap.at(vertexId); @@ -935,7 +931,7 @@ void DeformationGraph::computeVertexPosition(int vertexId, Eigen::Vector3f& posi position(1) = 0; position(2) = 0; - Eigen::Vector3f sourcePosition = sourceVertices->at(vertexId); + Eigen::Vector3d sourcePosition = sourceVertices->at(vertexId); for (uint32_t i = 0; i < weightMap.size(); i++) { position += weightMap.at(i).weight * @@ -950,7 +946,7 @@ float DeformationGraph::nonRelativeConstraintError() { for (uint32_t l = 0; l < constraints.size(); l++) { if (!constraints.at(l).relative) { - Eigen::Vector3f position; + Eigen::Vector3d position; computeVertexPosition(constraints.at(l).vertexId, position); result += (position - constraints.at(l).targetPosition).norm(); } diff --git a/Core/Utils/DeformationGraph.h b/Core/Utils/DeformationGraph.h index e5ae29b5..25c63825 100644 --- a/Core/Utils/DeformationGraph.h +++ b/Core/Utils/DeformationGraph.h @@ -20,6 +20,7 @@ #ifndef DEFORMATIONGRAPH_H_ #define DEFORMATIONGRAPH_H_ +#include #include #include "GraphNode.h" @@ -35,21 +36,17 @@ class CholeskyDecomp; class DeformationGraph { public: - DeformationGraph(int k, std::vector* sourceVertices); + DeformationGraph(int k, std::vector* sourceVertices); virtual ~DeformationGraph(); void initialiseGraph( - std::vector* customGraph, - std::vector* graphTimeMap); + std::vector* customGraph, + std::vector* graphTimeMap); - void appendVertices( - std::vector* vertexTimeMap, - uint32_t originalPointEnd); + void appendVertices(std::vector* vertexTimeMap, uint32_t originalPointEnd); // This clears the pose map... - void setPosesSeq( - std::vector* poseTimeMap, - const std::vector& poses); + void setPosesSeq(std::vector* poseTimeMap, const std::vector& T_wcs); // Stores a weight and node pointer for a vertex class VertexWeightMap { @@ -84,21 +81,21 @@ class DeformationGraph { }; std::vector& getGraph(); - std::vector& getGraphTimes(); + std::vector& getGraphTimes(); - void addConstraint(int vertexId, Eigen::Vector3f& target); + void addConstraint(int vertexId, Eigen::Vector3d& target); void addRelativeConstraint(int vertexId, int targetId); void clearConstraints(); void applyGraphToVertices(); - void applyGraphToPoses(std::vector& poses); + void applyGraphToPoses(std::vector T_wc_ptrs); bool optimiseGraphSparse( float& error, float& meanConsErr, const bool fernMatch, - const unsigned long long int lastDeformTime); + const uint64_t lastDeformTime); void resetGraph(); bool isInit() { @@ -127,7 +124,7 @@ class DeformationGraph { // Maps vertex indices to neighbours and weights std::vector> vertexMap; - std::vector* sourceVertices; + std::vector* sourceVertices; // Maps pose indices to neighbours and weights std::vector> poseMap; @@ -135,32 +132,32 @@ class DeformationGraph { // Stores a vertex constraint class Constraint { public: - Constraint(int vertexId, Eigen::Vector3f& targetPosition) + Constraint(int vertexId, Eigen::Vector3d& targetPosition) : vertexId(vertexId), targetPosition(targetPosition), relative(false), targetId(-1) {} Constraint(int vertexId, int targetId) : vertexId(vertexId), - targetPosition(Eigen::Vector3f::Zero()), + targetPosition(Eigen::Vector3d::Zero()), relative(true), targetId(targetId) {} int vertexId; - Eigen::Vector3f targetPosition; + Eigen::Vector3d targetPosition; bool relative; int targetId; }; std::vector constraints; - std::vector* graphCloud; - std::vector sampledGraphTimes; + std::vector* graphCloud; + std::vector sampledGraphTimes; uint32_t lastPointCount; void connectGraphSeq(); - void weightVerticesSeq(std::vector* vertexTimeMap); + void weightVerticesSeq(std::vector* vertexTimeMap); - void computeVertexPosition(int vertexId, Eigen::Vector3f& position); + void computeVertexPosition(int vertexId, Eigen::Vector3d& position); void sparseJacobian(Jacobian& jacobian, const int numRows, const int numCols, const int backSet); diff --git a/Core/Utils/GraphNode.h b/Core/Utils/GraphNode.h index 5d8f83ff..f23b356c 100644 --- a/Core/Utils/GraphNode.h +++ b/Core/Utils/GraphNode.h @@ -28,9 +28,9 @@ class GraphNode { GraphNode() {} int id; - Eigen::Vector3f position; - Eigen::Matrix3f rotation; - Eigen::Vector3f translation; + Eigen::Vector3d position; + Eigen::Matrix3d rotation; + Eigen::Vector3d translation; std::vector neighbours; bool enabled; }; diff --git a/Core/Utils/OdometryProvider.h b/Core/Utils/OdometryProvider.h index bd09fe8d..b7956a18 100644 --- a/Core/Utils/OdometryProvider.h +++ b/Core/Utils/OdometryProvider.h @@ -21,6 +21,7 @@ #define ODOMETRYPROVIDER_H_ #include +#include #include #include diff --git a/Core/Utils/RGBDOdometry.cpp b/Core/Utils/RGBDOdometry.cpp index e6e11bc7..aa7ded02 100644 --- a/Core/Utils/RGBDOdometry.cpp +++ b/Core/Utils/RGBDOdometry.cpp @@ -165,7 +165,7 @@ void RGBDOdometry::initICP(GPUTexture* predictedVertices, GPUTexture* predictedN void RGBDOdometry::initICPModel( GPUTexture* predictedVertices, GPUTexture* predictedNormals, - const Eigen::Matrix4f& modelPose) { + const Sophus::SE3d& T_wc) { cudaArray_t vmapPtr, nmapPtr; cudaGraphicsMapResources(1, &predictedVertices->cudaRes); @@ -184,18 +184,18 @@ void RGBDOdometry::initICPModel( resizeNMap(nmaps_g_prev_[i - 1], nmaps_g_prev_[i]); } - Eigen::Matrix Rcam = modelPose.topLeftCorner(3, 3); - Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1); + Eigen::Matrix R_wc = T_wc.rotationMatrix().cast(); + Eigen::Vector3f t_wc = T_wc.translation().cast(); - mat33 device_Rcam = Rcam; - float3 device_tcam = *reinterpret_cast(tcam.data()); + mat33 device_R_wc = R_wc; + float3 device_t_wc = *reinterpret_cast(t_wc.data()); for (int i = 0; i < NUM_PYRS; ++i) { tranformMaps( vmaps_g_prev_[i], nmaps_g_prev_[i], - device_Rcam, - device_tcam, + device_R_wc, + device_t_wc, vmaps_g_prev_[i], nmaps_g_prev_[i]); } @@ -251,8 +251,7 @@ void RGBDOdometry::initFirstRGB(GPUTexture* rgb) { } void RGBDOdometry::getIncrementalTransformation( - Eigen::Vector3f& trans, - Eigen::Matrix& rot, + Sophus::SE3d& T_wc, const bool& rgbOnly, const float& icpWeight, const bool& pyramid, @@ -261,8 +260,8 @@ void RGBDOdometry::getIncrementalTransformation( bool icp = !rgbOnly && icpWeight > 0; bool rgb = rgbOnly || icpWeight < 100; - Eigen::Matrix Rprev = rot; - Eigen::Vector3f tprev = trans; + Eigen::Matrix Rprev = T_wc.rotationMatrix().cast(); + Eigen::Vector3f tprev = T_wc.translation().cast(); Eigen::Matrix Rcurr = Rprev; Eigen::Vector3f tcurr = tprev; @@ -558,8 +557,188 @@ void RGBDOdometry::getIncrementalTransformation( } } - trans = tcurr; - rot = Rcurr; + Eigen::JacobiSVD svd( + Rcurr.cast(), Eigen::ComputeFullU | Eigen::ComputeFullV); + + T_wc.translation() = tcurr.cast(); + T_wc.setRotationMatrix(svd.matrixU() * svd.matrixV().transpose()); +} + +void RGBDOdometry::getIncrementalTransformationNew( + Eigen::Vector3f& trans, + Eigen::Matrix& rot, + const bool& /*rgbOnly*/, + const float& /*icpWeight*/, + const bool& pyramid, + const bool& fastOdom, + const bool& /*so3*/) { + // iterations[0] = fastOdom ? 3 : 10; + // iterations[1] = pyramid ? 5 : 0; + // iterations[2] = pyramid ? 4 : 0; + + // for (int i = NUM_PYRS - 1; i >= 0; i--) { + // Eigen::Matrix K = + // Eigen::Matrix::Zero(); + + // K(0, 0) = intr(i).fx; + // K(1, 1) = intr(i).fy; + // K(0, 2) = intr(i).cx; + // K(1, 2) = intr(i).cy; + // K(2, 2) = 1; + + // lastRGBError = std::numeric_limits::max(); + + // for (int j = 0; j < iterations[i]; j++) { + // Eigen::Matrix Rt = resultRt.inverse(); + + // Eigen::Matrix R = Rt.topLeftCorner(3, 3); + + // Eigen::Matrix KRK_inv = K * R * K.inverse(); + // mat33 krkInv; + // memcpy(&krkInv.data[0], KRK_inv.cast().eval().data(), sizeof(mat33)); + + // Eigen::Vector3d Kt = Rt.topRightCorner(3, 1); + // Kt = K * Kt; + // float3 kt = {(float)Kt(0), (float)Kt(1), (float)Kt(2)}; + + // int sigma = 0; + // int rgbSize = 0; + + // if (rgb) { + // TICK("computeRgbResidual"); + // computeRgbResidual( + // pow(minimumGradientMagnitudes[i], 2.0) / pow(sobelScale, 2.0), + // nextdIdx[i], + // nextdIdy[i], + // lastDepth[i], + // nextDepth[i], + // lastImage[i], + // nextImage[i], + // corresImg[i], + // sumResidualRGB, + // maxDepthDeltaRGB, + // kt, + // krkInv, + // sigma, + // rgbSize); + // TOCK("computeRgbResidual"); + // } + + // float sigmaVal = std::sqrt((float)sigma / rgbSize == 0 ? 1 : rgbSize); + // float rgbError = std::sqrt(sigma) / (rgbSize == 0 ? 1 : rgbSize); + + // if (rgbOnly && rgbError > lastRGBError) { + // break; + // } + + // lastRGBError = rgbError; + // lastRGBCount = rgbSize; + + // if (rgbOnly) { + // sigmaVal = -1; // Signals the internal optimisation to weight evenly + // } + + // Eigen::Matrix A_icp; + // Eigen::Matrix b_icp; + + // mat33 device_Rcurr = Rcurr; + // float3 device_tcurr = *reinterpret_cast(tcurr.data()); + + // DeviceArray2D& vmap_curr = vmaps_curr_[i]; + // DeviceArray2D& nmap_curr = nmaps_curr_[i]; + + // DeviceArray2D& vmap_g_prev = vmaps_g_prev_[i]; + // DeviceArray2D& nmap_g_prev = nmaps_g_prev_[i]; + + // float residual[2]; + + // if (icp) { + // TICK("icpStep"); + // icpStep( + // device_Rcurr, + // device_tcurr, + // vmap_curr, + // nmap_curr, + // device_Rprev_inv, + // device_tprev, + // intr(i), + // vmap_g_prev, + // nmap_g_prev, + // distThres_, + // angleThres_, + // sumDataSE3, + // outDataSE3, + // A_icp.data(), + // b_icp.data(), + // &residual[0]); + // TOCK("icpStep"); + // } + + // lastICPError = sqrt(residual[0]) / residual[1]; + // lastICPCount = residual[1]; + + // Eigen::Matrix A_rgbd; + // Eigen::Matrix b_rgbd; + + // if (rgb) { + // TICK("rgbStep"); + // rgbStep( + // corresImg[i], + // sigmaVal, + // pointClouds[i], + // intr(i).fx, + // intr(i).fy, + // nextdIdx[i], + // nextdIdy[i], + // sobelScale, + // sumDataSE3, + // outDataSE3, + // A_rgbd.data(), + // b_rgbd.data()); + // TOCK("rgbStep"); + // } + + // Eigen::Matrix result; + // Eigen::Matrix dA_rgbd = A_rgbd.cast(); + // Eigen::Matrix dA_icp = A_icp.cast(); + // Eigen::Matrix db_rgbd = b_rgbd.cast(); + // Eigen::Matrix db_icp = b_icp.cast(); + + // if (icp && rgb) { + // double w = icpWeight; + // lastA = dA_rgbd + w * w * dA_icp; + // lastb = db_rgbd + w * db_icp; + // result = lastA.ldlt().solve(lastb); + // } else if (icp) { + // lastA = dA_icp; + // lastb = db_icp; + // result = lastA.ldlt().solve(lastb); + // } else if (rgb) { + // lastA = dA_rgbd; + // lastb = db_rgbd; + // result = lastA.ldlt().solve(lastb); + // } else { + // assert(false && "Control shouldn't reach here"); + // } + + // Eigen::Isometry3f rgbOdom; + + // OdometryProvider::computeUpdateSE3(resultRt, result, rgbOdom); + + // Eigen::Isometry3f currentT; + // currentT.setIdentity(); + // currentT.rotate(Rprev); + // currentT.translation() = tprev; + + // currentT = currentT * rgbOdom.inverse(); + + // tcurr = currentT.translation(); + // Rcurr = currentT.rotation(); + // } + // } + + // trans = tcurr; + // rot = Rcurr; } Eigen::MatrixXd RGBDOdometry::getCovariance() { diff --git a/Core/Utils/RGBDOdometry.h b/Core/Utils/RGBDOdometry.h index 333b6817..4bfb3836 100644 --- a/Core/Utils/RGBDOdometry.h +++ b/Core/Utils/RGBDOdometry.h @@ -50,7 +50,7 @@ class RGBDOdometry { void initICPModel( GPUTexture* predictedVertices, GPUTexture* predictedNormals, - const Eigen::Matrix4f& modelPose); + const Sophus::SE3d& T_wc); void initRGB(GPUTexture* rgb); @@ -59,6 +59,14 @@ class RGBDOdometry { void initFirstRGB(GPUTexture* rgb); void getIncrementalTransformation( + Sophus::SE3d& T_wc, + const bool& rgbOnly, + const float& icpWeight, + const bool& pyramid, + const bool& fastOdom, + const bool& so3); + + void getIncrementalTransformationNew( Eigen::Vector3f& trans, Eigen::Matrix& rot, const bool& rgbOnly, diff --git a/Core/Utils/Stopwatch.h b/Core/Utils/Stopwatch.h index 303a0492..9ccc0a76 100644 --- a/Core/Utils/Stopwatch.h +++ b/Core/Utils/Stopwatch.h @@ -41,8 +41,8 @@ typedef uint8_t stopwatchPacketType; #ifndef DISABLE_STOPWATCH #define STOPWATCH(name, expression) \ do { \ - const unsigned long long int startTime = Stopwatch::getInstance().getCurrentSystemTime(); \ - expression const unsigned long long int endTime = \ + const uint64_t startTime = Stopwatch::getInstance().getCurrentSystemTime(); \ + expression const uint64_t endTime = \ Stopwatch::getInstance().getCurrentSystemTime(); \ Stopwatch::getInstance().addStopwatchTiming(name, endTime - startTime); \ } while (false) @@ -72,13 +72,13 @@ class Stopwatch { return instance; } - void addStopwatchTiming(std::string name, unsigned long long int duration) { + void addStopwatchTiming(std::string name, uint64_t duration) { if (duration > 0) { timings[name] = (float)(duration) / 1000.0f; } } - void setCustomSignature(unsigned long long int newSignature) { + void setCustomSignature(uint64_t newSignature) { signature = newSignature; } @@ -113,18 +113,18 @@ class Stopwatch { } } - static unsigned long long int getCurrentSystemTime() { + static uint64_t getCurrentSystemTime() { timeval tv; gettimeofday(&tv, 0); - unsigned long long int time = (unsigned long long int)(tv.tv_sec * 1000000 + tv.tv_usec); + uint64_t time = (uint64_t)(tv.tv_sec * 1000000 + tv.tv_usec); return time; } - void tick(std::string name, unsigned long long int start) { + void tick(std::string name, uint64_t start) { tickTimings[name] = start; } - void tock(std::string name, unsigned long long int end) { + void tock(std::string name, uint64_t end) { float duration = (float)(end - tickTimings[name]) / 1000.0f; if (duration > 0) { @@ -152,7 +152,7 @@ class Stopwatch { } stopwatchPacketType* serialiseTimings(int& packetSize) { - packetSize = sizeof(int) + sizeof(unsigned long long int); + packetSize = sizeof(int) + sizeof(uint64_t); for (std::map::const_iterator it = timings.begin(); it != timings.end(); it++) { @@ -163,9 +163,9 @@ class Stopwatch { dataPacket[0] = packetSize * sizeof(uint8_t); - *((unsigned long long int*)&dataPacket[1]) = signature; + *((uint64_t*)&dataPacket[1]) = signature; - float* valuePointer = (float*)&((unsigned long long int*)&dataPacket[1])[1]; + float* valuePointer = (float*)&((uint64_t*)&dataPacket[1])[1]; for (std::map::const_iterator it = timings.begin(); it != timings.end(); it++) { @@ -178,11 +178,11 @@ class Stopwatch { timeval clock; long long int currentSend, lastSend; - unsigned long long int signature; + uint64_t signature; int sockfd; struct sockaddr_in servaddr; std::map timings; - std::map tickTimings; + std::map tickTimings; }; #endif /* STOPWATCH_H_ */ diff --git a/MainController.cpp b/MainController.cpp index fca70629..ec139678 100644 --- a/MainController.cpp +++ b/MainController.cpp @@ -232,19 +232,18 @@ void MainController::run() { framesToSkip = 0; } - Eigen::Matrix4f* currentPose = 0; + Sophus::SE3d* T_wc = 0; if (groundTruthOdometry) { - currentPose = new Eigen::Matrix4f; - currentPose->setIdentity(); - *currentPose = groundTruthOdometry->getTransformation(logReader->timestamp); + T_wc = new Sophus::SE3d( + groundTruthOdometry->getTransformation(logReader->timestamp).cast()); } eFusion->processFrame( - logReader->rgb, logReader->depth, logReader->timestamp, currentPose, weightMultiplier); + logReader->rgb, logReader->depth, logReader->timestamp, weightMultiplier, T_wc); - if (currentPose) { - delete currentPose; + if (T_wc) { + delete T_wc; } if (frameskip && Stopwatch::getInstance().getTimings().at("Run") > 1000.f / 30.f) { @@ -260,17 +259,17 @@ void MainController::run() { if (gui->followPose->Get()) { pangolin::OpenGlMatrix mv; - Eigen::Matrix4f currPose = eFusion->getCurrPose(); - Eigen::Matrix3f currRot = currPose.topLeftCorner(3, 3); + Eigen::Matrix4f T_wc = eFusion->get_T_wc().matrix().cast(); + Eigen::Matrix3f R_wc = T_wc.topLeftCorner(3, 3); - Eigen::Quaternionf currQuat(currRot); + Eigen::Quaternionf currQuat(R_wc); Eigen::Vector3f forwardVector(0, 0, 1); Eigen::Vector3f upVector(0, iclnuim ? 1 : -1, 0); Eigen::Vector3f forward = (currQuat * forwardVector).normalized(); Eigen::Vector3f up = (currQuat * upVector).normalized(); - Eigen::Vector3f eye(currPose(0, 3), currPose(1, 3), currPose(2, 3)); + Eigen::Vector3f eye(T_wc(0, 3), T_wc(1, 3), T_wc(2, 3)); eye -= forward; @@ -311,7 +310,7 @@ void MainController::run() { gui->inLog.Log(eFusion->getModelToModel().lastICPCount, icpCountThresh); } - Eigen::Matrix4f pose = eFusion->getCurrPose(); + const Eigen::Matrix4f T_wc = eFusion->get_T_wc().matrix().cast(); if (gui->drawRawCloud->Get() || gui->drawFilteredCloud->Get()) { eFusion->computeFeedbackBuffers(); @@ -322,7 +321,7 @@ void MainController::run() { .at(FeedbackBuffer::RAW) ->render( gui->s_cam.GetProjectionModelViewMatrix(), - pose, + T_wc, gui->drawNormals->Get(), gui->drawColors->Get()); } @@ -332,7 +331,7 @@ void MainController::run() { .at(FeedbackBuffer::FILTERED) ->render( gui->s_cam.GetProjectionModelViewMatrix(), - pose, + T_wc, gui->drawNormals->Get(), gui->drawColors->Get()); } @@ -372,7 +371,7 @@ void MainController::run() { } else { glColor3f(1, 0, 1); } - gui->drawFrustum(pose); + gui->drawFrustum(T_wc); glColor3f(1, 1, 1); if (gui->drawFerns->Get()) { @@ -381,7 +380,7 @@ void MainController::run() { if ((int)i == eFusion->getFerns().lastClosest) continue; - gui->drawFrustum(eFusion->getFerns().frames.at(i)->pose); + gui->drawFrustum(eFusion->getFerns().frames.at(i)->T_wc.cast().matrix()); } glColor3f(1, 1, 1); } @@ -407,7 +406,10 @@ void MainController::run() { if (eFusion->getFerns().lastClosest != -1) { glColor3f(1, 0, 0); - gui->drawFrustum(eFusion->getFerns().frames.at(eFusion->getFerns().lastClosest)->pose); + gui->drawFrustum(eFusion->getFerns() + .frames.at(eFusion->getFerns().lastClosest) + ->T_wc.cast() + .matrix()); glColor3f(1, 1, 1); } diff --git a/Tools/GroundTruthOdometry.cpp b/Tools/GroundTruthOdometry.cpp index 8c12dab1..86584cc7 100644 --- a/Tools/GroundTruthOdometry.cpp +++ b/Tools/GroundTruthOdometry.cpp @@ -30,11 +30,11 @@ void GroundTruthOdometry::loadTrajectory(const std::string& filename) { std::string line; file.open(filename.c_str()); while (!file.eof()) { - unsigned long long int utime; + uint64_t utime; float x, y, z, qx, qy, qz, qw; std::getline(file, line); int n = - sscanf(line.c_str(), "%llu,%f,%f,%f,%f,%f,%f,%f", &utime, &x, &y, &z, &qx, &qy, &qz, &qw); + sscanf(line.c_str(), "%lu,%f,%f,%f,%f,%f,%f,%f", &utime, &x, &y, &z, &qx, &qy, &qz, &qw); if (file.eof()) break; diff --git a/third-party/Sophus b/third-party/Sophus new file mode 160000 index 00000000..26c20026 --- /dev/null +++ b/third-party/Sophus @@ -0,0 +1 @@ +Subproject commit 26c200265e2eb3d76e5ab00a99ada686d6a80d15