 		pg23[i] = o_x * j_ang[3] + o_y * j_ang[4] + o_z * j_ang[5];
 		pg04[i] = o_x * j_ang[6] + o_y * j_ang[7] + o_z * j_ang[8];
 		pg14[i] = o_x * j_ang[9] + o_y * j_ang[10] + o_z * j_ang[11];
-
-		pg24[i] = o_x * j_ang[12] + o_y * j_ang[13] + o_z * j_ang[14];
-		pg05[i] = o_x * j_ang[15] + o_y * j_ang[16] + o_z * j_ang[17];
-		pg15[i] = o_x * j_ang[18] + o_y * j_ang[19] + o_z * j_ang[20];
-		pg25[i] = o_x * j_ang[21] + o_y * j_ang[22] + o_z * j_ang[23];
+	}
+}
+
+/* Second step of computing point gradients */
+__global__ void computePointGradients1(float *x, float *y, float *z, int points_num,
+													int *valid_points, int valid_points_num,
+													double *dj_ang,
+													double *pg24, double *pg05, double *pg15, double *pg25)
+{
+	int id = threadIdx.x + blockIdx.x * blockDim.x;
+	int stride = blockDim.x * gridDim.x;
+	__shared__ double j_ang[12];
+
+
+	if (threadIdx.x < 12) {
+		j_ang[threadIdx.x] = dj_ang[threadIdx.x + 12];
+	}
+
+	__syncthreads();
+
+	for (int i = id; i < valid_points_num; i += stride) {
+		int pid = valid_points[i];
+
+		//Orignal coordinates
+		double o_x = static_cast<double>(x[pid]);
+		double o_y = static_cast<double>(y[pid]);
+		double o_z = static_cast<double>(z[pid]);
+
+		//Compute point derivatives
+
+		pg24[i] = o_x * j_ang[0] + o_y * j_ang[1] + o_z * j_ang[2];
+		pg05[i] = o_x * j_ang[3] + o_y * j_ang[4] + o_z * j_ang[5];
+		pg15[i] = o_x * j_ang[6] + o_y * j_ang[7] + o_z * j_ang[8];
+		pg25[i] = o_x * j_ang[9] + o_y * j_ang[10] + o_z * j_ang[11];
 	}
 }
 

												point_gradients + valid_points_num * 9,
												point_gradients + valid_points_num * 15,
												point_gradients + valid_points_num * 4,
												point_gradients + valid_points_num * 10,
												point_gradients + valid_points_num * 16,
												point_gradients + valid_points_num * 5,
												point_gradients + valid_points_num * 11,

	}
}

void GNormalDistributionsTransform::setInputTarget(pcl::PointXYZI *input, int size)
{
	// Copy input map data from the host memory to the GPU memory
	GRegistration::setInputTarget(input, size);

	// Build the voxel grid
	if (target_points_number_ != 0) {
		voxel_grid_.setLeafSize(resolution_, resolution_, resolution_);
		voxel_grid_.setInput(target_x_, target_y_, target_z_, target_points_number_);
	}
}

void GNormalDistributionsTransform::setInputTarget(pcl::PointXYZ *input, int size)
{
	// Copy input map data from the host memory to the GPU memory
	GRegistration::setInputTarget(input, size);

	// Build the voxel grid
	if (target_points_number_ != 0) {
		voxel_grid_.setLeafSize(resolution_, resolution_, resolution_);
		voxel_grid_.setInput(target_x_, target_y_, target_z_, target_points_number_);
	}
}

void GNormalDistributionsTransform::computeTransformation(const Eigen::Matrix<float, 4, 4> &guess)
{

