cyclical_buffer.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-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.
 *
 */
 /*
  * cyclical_buffer.h
  *
  *  @date 13.11.2015
  *  @author Tristan Igelbrink (Tristan@Igelbrink.com)
  */


#ifndef CYCLICAL_BUFFER_IMPL_H_
#define CYCLICAL_BUFFER_IMPL_H_

#include <kfusion/cuda/tsdf_volume.hpp>
#include <kfusion/tsdf_buffer.h>
#include <kfusion/LVRPipeline.hpp>
#include <Eigen/Core>
#include "types.hpp"
#include <cuda_runtime.h>
#include <thread>

namespace kfusion
{
    namespace cuda
    {
                class KF_EXPORTS CyclicalBuffer
                {

                  public:
                        CyclicalBuffer (KinFuParams params): pl_(params),
                                        optimize_(params.cmd_options->optimizePlanes()), no_reconstruct_(params.cmd_options->noReconstruction())
                        {
                                distance_threshold_ = params.shifting_distance;
                                buffer_.volume_size.x = params.volume_size[0];
                                buffer_.volume_size.y = params.volume_size[1];
                                buffer_.volume_size.z = params.volume_size[2];
                                buffer_.voxels_size.x = params.volume_dims[0];
                                buffer_.voxels_size.y = params.volume_dims[1];
                                buffer_.voxels_size.z = params.volume_dims[2];
                                global_shift_[0] = 0;
                                global_shift_[1] = 0;
                                global_shift_[2] = 0;
                        }


                        /*CyclicalBuffer (const double distance_threshold,
                                        const double volume_size_x, const double volume_size_y,
                                        const double volume_size_z, const int nb_voxels_x, const int nb_voxels_y,
                                        const int nb_voxels_z)
                        {
                                distance_threshold_ = distance_threshold;
                                buffer_.volume_size.x = volume_size_x;
                                buffer_.volume_size.y = volume_size_y;
                                buffer_.volume_size.z = volume_size_z;
                                buffer_.voxels_size.x = nb_voxels_x;
                                buffer_.voxels_size.y = nb_voxels_y;
                                buffer_.voxels_size.z = nb_voxels_z;
                        }*/

                        ~CyclicalBuffer()
                        {
                                //double averageMCTime = mcwrap_.calcTimeStats();
                                //cout << "----- Average time for processing one tsdf value " << averageMCTime << "ns -----" << endl;
                        }

                        bool checkForShift (cv::Ptr<cuda::TsdfVolume> volume,
                                            const Affine3f &cam_pose, const double distance_camera_target,
                                            const bool perform_shift = true, const bool last_shift = false,
                                            const bool record_mode = false);

                        void performShift (cv::Ptr<cuda::TsdfVolume> volume, const cv::Vec3f& target_point,  const Affine3f &cam_pose, const bool last_shift = false, const bool record_mode = false);

                    void setDistanceThreshold (const double threshold)
                    {
                          distance_threshold_ = threshold;
                    }

                    float getDistanceThreshold () { return (distance_threshold_); }

                    tsdf_buffer& getBuffer () { return (buffer_); }

                        void setVolumeSize (const Vec3f size)
                        {
                                buffer_.volume_size.x = size[0];
                                buffer_.volume_size.y = size[1];
                                buffer_.volume_size.z = size[2];
                        }

                        void setVoxelSize (const Vec3f vsize)
                        {
                                buffer_.voxels_size.x = vsize[0];
                                buffer_.voxels_size.y = vsize[1];
                                buffer_.voxels_size.z = vsize[2];
                        }

                        Vec3f getOrigin()
                        {
                                return Vec3f(buffer_.origin_metric.x, buffer_.origin_metric.y, buffer_.origin_metric.z);
                        }

                  void setVolumeSize (const double size)
                  {
                        buffer_.volume_size.x = size;
                        buffer_.volume_size.y = size;
                        buffer_.volume_size.z = size;
                  }

                  void computeAndSetNewCubeMetricOrigin (cv::Ptr<cuda::TsdfVolume> volume, const cv::Vec3f& target_point, Vec3i& offset);

                  void initBuffer (cv::Ptr<cuda::TsdfVolume> tsdf_volume)
                  {
                        buffer_.tsdf_memory_start = tsdf_volume->getCoord(0, 0, 0, 0, 0);
                        buffer_.tsdf_memory_end = tsdf_volume->getCoord(buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1, buffer_.voxels_size.x, buffer_.voxels_size.y);

                        buffer_.tsdf_rolling_buff_origin = buffer_.tsdf_memory_start;
                  }

                  void resetBuffer (cv::Ptr<cuda::TsdfVolume> tsdf_volume)
                  {
                        buffer_.origin_GRID.x = 0; buffer_.origin_GRID.y = 0; buffer_.origin_GRID.z = 0;
                        buffer_.origin_GRID_global.x = 0.f; buffer_.origin_GRID_global.y = 0.f; buffer_.origin_GRID_global.z = 0.f;
                        Vec3f position = tsdf_volume->getPose().translation();
                        buffer_.origin_metric.x = position[0];
                        buffer_.origin_metric.y = position[1];
                        buffer_.origin_metric.z = position[2];
                        initBuffer (tsdf_volume);
                  }

                  void resetMesh(){/*mcwrap_.resetMesh();*/}
                  void addImgPose(ImgPose* imgPose){ imgPoses_.push_back(imgPose);}

                  MeshPtr getMesh() {return pl_.getMesh();}

                  int getSliceCount(){return slice_count_;}


                private:

                  DeviceArray<Point> cloud_buffer_device_;

                  double distance_threshold_;
                  Vec3i global_shift_;
                  cv::Mat cloud_slice_;
                  std::vector<ImgPose*> imgPoses_;
                  int slice_count_ = 0;
                  Affine3f last_camPose_;
                  bool optimize_, no_reconstruct_;

                  tsdf_buffer buffer_;

                  //MaCuWrapper mcwrap_;
                  LVRPipeline pl_;
                        inline int calcIndex(float f) const
                        {
                                return f < 0 ? f-.5:f+.5;
                        }

                  float euclideanDistance(const Point& p1, const Point& p2);

                  void getEulerYPR(float& yaw, float& pitch, float& roll, cv::Affine3<float>::Mat3& mat,  unsigned int solution_number = 1);

                  void calcBounds(Vec3i& offset, Vec3i& minBounds, Vec3i& maxBounds);

                  void shiftOrigin (cv::Ptr<cuda::TsdfVolume> tsdf_volume, Vec3i offset)
                  {
                        // shift rolling origin (making sure they keep in [0 - NbVoxels[ )
                        buffer_.origin_GRID.x += offset[0];
                        if(buffer_.origin_GRID.x >= buffer_.voxels_size.x)
                          buffer_.origin_GRID.x -= buffer_.voxels_size.x;
                        else if(buffer_.origin_GRID.x < 0)
                        {
                          buffer_.origin_GRID.x += buffer_.voxels_size.x ;
                        }

                        buffer_.origin_GRID.y += offset[1];
                        if(buffer_.origin_GRID.y >= buffer_.voxels_size.y)
                          buffer_.origin_GRID.y -= buffer_.voxels_size.y;
                        else if(buffer_.origin_GRID.y < 0)
                        {
                          buffer_.origin_GRID.y += buffer_.voxels_size.y;
                        }

                        buffer_.origin_GRID.z += offset[2];
                        if(buffer_.origin_GRID.z >= buffer_.voxels_size.z)
                          buffer_.origin_GRID.z -= buffer_.voxels_size.z;
                        else if(buffer_.origin_GRID.z < 0)
                        {
                          buffer_.origin_GRID.z += buffer_.voxels_size.z;
                        }
                        // update memory pointers
                        CudaData localVolume = tsdf_volume->data();
                        buffer_.tsdf_memory_start = tsdf_volume->getCoord(0, 0, 0, 0, 0);
                        buffer_.tsdf_memory_end = tsdf_volume->getCoord(buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1, buffer_.voxels_size.x, buffer_.voxels_size.y);
                        buffer_.tsdf_rolling_buff_origin = tsdf_volume->getCoord(buffer_.origin_GRID.x, buffer_.origin_GRID.y, buffer_.origin_GRID.z, buffer_.voxels_size.x, buffer_.voxels_size.y);

                        // update global origin
                        global_shift_[0] = buffer_.origin_GRID_global.x += offset[0];
                        global_shift_[1] = buffer_.origin_GRID_global.y += offset[1];
                        global_shift_[2] = buffer_.origin_GRID_global.z += offset[2];
                  }

          };
        }
}

#endif // CYCLICAL_BUFFER_IMPL_H_