integrateCloud segmentation faullt

[MiroslavKohut]

Hi

I am using your program for generating TSDF volumes. I had pointclouds from 3d Scanner calibrated on the robot. I also have transformation matrixes of the camera frame in world coorditane system. When I try to call your integrateCloud method I always get segmentation fault.
Here is my code

/*
 * Copyright (c) 2013-, Stephen Miller
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, 
 * with or without modification, are permitted provided 
 * that the following conditions are met:
 * 
 * 1. Redistributions of source code must retain the 
 * above copyright notice, this list of conditions 
 * and the following disclaimer.
 *
 * 2. 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.
 * 
 * 3. Neither the name of the copyright holder 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 HOLDER 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.
 */


#include <pcl/console/print.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/pcl_macros.h>

#include <boost/filesystem/convenience.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <string>
#include <vector>

#include <cpu_tsdf/tsdf_interface.h>
#include <cpu_tsdf/tsdf_volume_octree.h>
#include <cpu_tsdf/marching_cubes_tsdf_octree.h>


using  namespace cpu_tsdf;

int width_ = 2064;
int height_= 1544;

int main (int argc, char** argv)
{

    cpu_tsdf::TSDFVolumeOctree::Ptr tsdf (new TSDFVolumeOctree);
     tsdf->setGridSize (10., 10., 10.); // 10m x 10m x 10m
     tsdf->setResolution (4096, 4096, 4096); // Smallest cell size = 10m / 2048 = about half a centimeter
     tsdf->setIntegrateColor (false); // Set to true if you want the TSDF to store color
     tsdf->setImageSize (width_, height_);
     //Eigen::Affine3d tsdf_center; // Optionally offset the center
     //tsdf->setGlobalTransform (tsdf_center);
     tsdf->reset (); // Initialize it to be empty

     std::vector<pcl::PointCloud<pcl::PointXYZRGBA>::Ptr> clouds;
     std::vector<Eigen::Affine3d> transformations;

     for (size_t i = 1; i < 12; i++)
     {

         //int width_ = 640;
        // int height_ = 480;
         pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGBA>);
         pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_organized (new pcl::PointCloud<pcl::PointXYZRGBA> (width_, height_));

         if (pcl::io::loadPLYFile<pcl::PointXYZRGBA>(
                 "/home/mirec/catkin_ws/clouds/cloud_from_phoxi/cloud" + std::to_string(i) + ".ply",
                 *cloud_out) == -1) //* load the file
         {
             PCL_ERROR ("Couldn't read file I! \n");
             return (-1);
         }
         else {
             //scale
             double N = 0.001;
             Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
             transform (0,0) = transform (0,0) * N;
             transform (1,1) = transform (1,1) * N;
             transform (2,2) = transform (2,2) * N;
             pcl::transformPointCloud (*cloud_out, *cloud_out, transform);

             for (long int x=0;x<= cloud_out->size();x++){

                     cloud_organized->points[x] = cloud_out->points[x];

             }


             pcl::io::savePLYFileBinary("/home/mirec/catkin_ws/clouds/cloud_from_phoxi/clouda" + std::to_string(i) + ".ply",*cloud_organized);
             std::cout << "VEELKOST:" << cloud_organized->width << std::endl;

             std::ifstream input_file1;
             std::ifstream input_file2;

             input_file1.open("/home/mirec/catkin_ws/clouds/point_for_phoxi/cloud" + std::to_string(i) + ".txt");
             if (!input_file1) {
                 std::cout << "ERROR OPENING FILE /home/mirec/catkin_ws/clouds/point_for_phoxi/cloud" + std::to_string(i) + ".txt"<< std::endl;
                 return false;
             }
             Eigen::Quaterniond rot;
             Eigen::Vector3d translation;

             input_file1 >> translation.x() >> translation.y() >> translation.z();
             input_file1 >> rot.x() >> rot.y() >> rot.z() >> rot.w();

             Eigen::Matrix4d transformation_matrix;

             Eigen::Matrix3d rotacia(rot.toRotationMatrix());
             std::cout << rotacia << std::endl;

             for (int i = 0; i<3;i++){
                 for (int x = 0; x<3;x++){
                     transformation_matrix(i,x) = rotacia(i,x);
                 }
             }

             transformation_matrix(0,3) = translation.x();
             transformation_matrix(1,3) = translation.y();
             transformation_matrix(2,3) = translation.z();
             transformation_matrix(3,3) = 1;
             transformation_matrix(3,0) = 0;
             transformation_matrix(3,1) = 0;
             transformation_matrix(3,2) = 0;


             std::cout << transformation_matrix << std::endl;


             Eigen::Affine3d transformation;
             transformation = transformation_matrix;
             clouds.push_back(cloud_organized);
             transformations.push_back(transformation);
             //tsdf->integrateCloud (*cloud_organized, pcl::PointCloud<pcl::Normal>(), transformation);
         }

     }

    for (size_t i = 0; i < clouds.size (); i++)
    {
        tsdf->integrateCloud (*clouds[i], pcl::PointCloud<pcl::Normal>(), transformations[i]);
        // Integrate the cloud
        // Note, the normals aren't being used in the default settings. Feel free to pass in an empty cloud
    }
     // Now what do you want to do with it?
     float distance; pcl::PointXYZ query_point (1.0, 2.0, -1.0);
     tsdf->getFxn (query_point, distance); // distance is normalized by the truncation limit -- goes from -1 to 1
     //pcl::PointCloud<pcl::PointNormal>::Ptr raytraced = tsdf->renderView (pose_to_render_from); // Optionally can render it
     tsdf->save ("output.vol"); // Save it?

     // Mesh with marching cubes
     MarchingCubesTSDFOctree mc;
     mc.setInputTSDF (tsdf);
     mc.setMinWeight (2); // Sets the minimum weight -- i.e. if a voxel sees a point less than 2 times, it will not render  a mesh triangle at that location
     mc.setColorByRGB (false); // If true, tries to use the RGB values of the TSDF for meshing -- required if you want a colored mesh
     pcl::PolygonMesh mesh;
     mc.reconstruct (mesh);
}

Here is GDB output

please can you help me what am I doing wrong

Thread 1 "dp_test" received signal SIGSEGV, Segmentation fault.
__GI___libc_free (mem=0xa1) at malloc.c:2951
2951 malloc.c: No such file or directory.
(gdb) bt
#0 __GI___libc_free (mem=0xa1) at malloc.c:2951
#1 0x00007ffff5884244 in cpu_tsdf::TSDFVolumeOctree::getFrustumCulledVoxels(Eigen::Transform<double, 3, 2, 0> const&, std::vector<boost::shared_ptr<cpu_tsdf::OctreeNode>, std::allocator<boost::shared_ptr<cpu_tsdf::OctreeNode> > >&) const
() from /home/mirec/cpu_tsdf/build/libcpu_tsdf.so
#2 0x0000000000411c23 in bool cpu_tsdf::TSDFVolumeOctree::integrateCloud<pcl::PointXYZRGBA, pcl::Normal>(pcl::PointCloudpcl::PointXYZRGBA const&, pcl::PointCloudpcl::Normal const&, Eigen::Transform<double, 3, 2, 0> const&) ()
#3 0x000000000040dd8e in main ()

Thank you for your time

[sdmiller]

Hi @MiroslavKohut ,

At first glance I don't see anything being misused, and can't really think of why my code would have a libc_free error here. Is this crash happening on the very first call to integrateCloud, or some time into the process?

One thing I notice, which shouldn't trigger a segfault but might confuse the algorithm, is that you don't seem to be passing in intrinsic information. With a 2064 x 1544 image, I'd be surprised if the default kinect focal length is going to be close. Another thing I'm curious about, is if this organized cloud is actually "organized" -- is it dense (i.e., are there exactly 2064x1544 points in row major order), or is this copy operation simply putting the points in a random location? Note in my integrate.cpp script, I'm doing the projective math to make sure cloud(x,y) = the point corresponding to pixel x,y. Note, also, that the accessor (*cloud)(x,y) = point is safer than directly modifying cloud->points, as PCL is probably doing some other stuff on the side.

A few random hunches for the segfault would be: some mistmatch in boost versions (i.e. I depend on boost, but so does PCL; worth checking), or this rather esoteric Eigen alignment issue (which I remember being a problem in grad school sometimes): https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html . It'd be worth trying to guard that std::vector

[MiroslavKohut]

Hi thanky you for your time and soon answer

The problem occurred at the begging. After I load data and call integrateCloud for the first time the program crashes. I also tried to put only empty organized cloud to the function and it crashed either. Thats why I dont think there is problem with my cloud data. Guarding std::vector of Eigen matrices did not help.

[sdmiller]

Could you do a quick check and see if my own integrate binary segfaults? That would help let us know if it has to do with clashing symbols / linker issues related to boost.

[MiroslavKohut]

Your integrate binary works fine I was able to create output from you sample data

[MiroslavKohut]

Ok I find a solution a resaved my data to pcd binary file and used your program for generating mesh :) it works without any problems. I still dont know where could be the problem in my solution but never mind. Thanks for your time :) and great cpu_tsdf package

[MiroslavKohut]

I have one more questie here are my data that i get after transformation of clouds in my program

And here is you output Is there a way to improve final mesh.ply ??

[MiroslavKohut]

I made it looks nicer with manually increasing resolution to 8192 but it still throws away too much data as you can see on the first picture boxes has all the sides. Do I need to insert bigger dataset ? or is there any other problem

[sdmiller]

Hmm, I would hazard a guess this has to do with intrinsics not being right? It seems to be projecting quite roughly -- I've found that this can sometimes happen with high resolution images, since the odds of a voxel reprojecting perfectly at a given pixel get much lower. More clouds should definitely help debug the problem :)

[MiroslavKohut]

Yeah it helped. Thank you very much for your time and help :)

[sdmiller]

If you try combinations of the --visualize and --cloud-only options in my integrate script, it might help debug the problem a bit -- showcases my aligned clouds (to ensure they're aligning right), and/or lets you step through the integration pipeline frame by frame.