Merge branch 'master' of github.com:isikmustafa/kinectfusion-cuda

enjoybo · Jan 30, 2019 · 359e6be · 359e6be
2 parents 637154e + 1ede3b4
commit 359e6be
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 4 deletions.
diff --git a/project/src/icp.cpp b/project/src/icp.cpp
@@ -4,6 +4,7 @@
 
 #include "cuda_utils.h"
 #include "icp.cuh"
+#include <glm/gtx/euler_angles.hpp>
 
 ICP::ICP(IcpConfig &config)
     : m_distance_thresh(config.distance_thresh)
@@ -46,6 +47,8 @@ RigidTransform3D ICP::computePose(GridMapPyramid<CudaGridMap> &vertex_pyramid,
 
             updatePose(pose_estimate);
 
+			//std::cout << "ICP residual count: " << countResiduals(grid_dims[0] * grid_dims[1]) / (float)(grid_dims[0] * grid_dims[1]) << std::endl;
+
             // If ICP converged, move directly to the next pyramid layer
             if (solver.getLastError() < m_stop_thresh)
             {
@@ -91,9 +94,7 @@ void ICP::updatePose(RigidTransform3D &pose)
 
 glm::mat3x3 ICP::buildRotationZYX(float z_angle, float y_angle, float x_angle)
 {
-    return  glm::mat3x3(glm::rotate(z_angle, glm::vec3(0.0f, 0.0f, 1.0f))
-                      * glm::rotate(y_angle, glm::vec3(0.0f, 1.0f, 0.0f))
-                      * glm::rotate(x_angle, glm::vec3(1.0f, 0.0f, 0.0f)));
+	return glm::orientate3(glm::vec3(x_angle, z_angle, y_angle));
 }
 
 unsigned int ICP::countResiduals(unsigned int max_idx)

diff --git a/project/src/raycast.cu b/project/src/raycast.cu
@@ -28,7 +28,7 @@ __device__ glm::vec2 intersectBBox(const glm::vec3& origin, const glm::vec3& inv
 
 __device__ glm::vec3 computeGradient(const glm::vec3& point, const VoxelGridStruct& voxel_grid)
 {
-	auto uvw_resolution = 0.5f / (voxel_grid.n - 1);
+	auto uvw_resolution = voxel_grid.mue * 0.5f / voxel_grid.total_width_in_meters;
 	auto uvw = point / voxel_grid.total_width_in_meters + glm::vec3(0.5f);
 
 	auto f_x0 = tex3D<float2>(voxel_grid.texture_object, uvw.x + uvw_resolution, uvw.y, uvw.z).x;