mp3guy / icpcuda Goto Github PK
View Code? Open in Web Editor NEWSuper fast implementation of ICP in CUDA for compute capable devices 3.5 or higher
License: BSD 3-Clause "New" or "Revised" License
Super fast implementation of ICP in CUDA for compute capable devices 3.5 or higher
License: BSD 3-Clause "New" or "Revised" License
Changes to pangolin seem to have broken this.
ICP.cpp line 58
depth.RowPtr(i)[j] = depthRaw.Reinterpret<unsigned short>().RowPtr(i)[j] / 5;
depthRaw is Image<unsigned char>
and Reinterpret now fails if the two data types are not the same length (oddly).
Pangolin/include/pangolin/image/image.h line 360:
PANGO_ASSERT(sizeof(TRecast) == sizeof(T), "sizeof(TRecast) must match sizeof(T): % != %", sizeof(TRecast), sizeof(T) );
This would seem to be a limitation of Pangolin if I'm reading it correctly but should probably be handled.
Hi, I am getting following Eigen errors when compiling the project:
Error 16 error : calling a host function("Eigen::DenseBase<Eigen::Matrix<float, (int)29, (int)1, (int)2, (int)29, (int)1> > ::Zero") from a device function("blockReduceSum<(int)29> ") is not allowed
Error 73 error : calling a host function("Eigen::DenseCoeffsBase<Eigen::Matrix<float, (int)29, (int)1, (int)2, (int)29, (int)1> , (int)1> ::operator []") from a device function("Reduction::SquareUpperTriangularProduct<(int)0, (int)0, (int)6> ::apply") is not allowed
Error 18 error : calling a host function("Eigen::DenseCoeffsBase<Eigen::Matrix<float, (int)29, (int)1, (int)2, (int)29, (int)1> , (int)1> ::operator []") from a device function("warpReduceSum<(int)29> ") is not allowed
Error 25 error : calling a host function("Eigen::Matrix<float, (int)29, (int)1, (int)2, (int)29, (int)1> ::Matrix") from a device function("Reduction::operator ()") is not allowed
Error 15 error : calling a host function("Eigen::Matrix<float, (int)29, (int)1, (int)2, (int)29, (int)1> ::MatrixEigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float , Eigen::Matrix<float, (int)29, (int)1, (int)2, (int)29, (int)1> > > ") from a device function("blockReduceSum<(int)29> ") is not allowed
Error 51 error : calling a host function("Eigen::MatrixBase<Eigen::Matrix<float, (int)3, (int)3, (int)2, (int)3, (int)3> > ::operator *<Eigen::Matrix<float, (int)3, (int)1, (int)2, (int)3, (int)1> > ") from a device function("Reduction::operator ()") is not allowed
I have built and installed Eigen from source files (CUDA supported). Tried using both github.com/stevenlovegrove/eigen and Eigen 3.3. Also I have installed Eigen, Pangolin, Sophus and ICPCUDA all in Release mode for x64 platform.
Any suggestions ?
Hi, thanks for making this code available.
I need to match two clouds (currently vectors of points), where one is a Subset of the other.
Is this implementation suitable for this? Will it match the smaller cloud to a section of the larger?
Or does it need both clouds to be the same size?
Thanks again.
Hi,
I tried to modify the code to enable it to take 3D point clouds as input -- however, when the relative distance between two cameras is large (distance ~0.4 m), this code returns abnormal results with the default dist_thresh and angle_thresh, I have to manually set dist_thresh to 0.6 m, angle_thresh to 20 degree to acquire reasonable results. Any comments on this? Thanks!
/home/SENSETIME/duanyongli/Software/ICPCUDA/third-party/Eigen/Eigen/src/Core/MathFunctions.h(1328): warning: calling a host function from a host device function is not allowed
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(75): error: identifier "sum" is undefined
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(76): error: identifier "out" is undefined
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(77): error: type name is not allowed
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(77): error: identifier "matrixA_host" is undefined
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(77): error: type name is not allowed
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(77): error: identifier "vectorB_host" is undefined
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(77): error: too many arguments for class template "Eigen::Matrix"
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(78): error: expected a ">"
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(78): error: expected a ">"
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(78): error: expected a ">"
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(75): error: identifier "sum" is undefined
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(76): error: identifier "out" is undefined
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(77): error: type name is not allowed
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(77): error: identifier "matrixA_host" is undefined
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(77): error: type name is not allowed
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(77): error: identifier "vectorB_host" is undefined
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(77): error: too many arguments for class template "Eigen::Matrix"
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(78): error: expected a ">"
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(78): error: expected a ">"
/home/SENSETIME/duanyongli/Software/ICPCUDA/Cuda/internal.h(78): error: expected a ">"
thanks a lot.
how to vizualize the output?
My devices:
GeForce GTX 765M
Ubuntu 1404
Installing is good, and when I run the test:
./ICP ~/rgbd_dataset_freiburg1_desk/ -v
GeForce GTX 765M
Searching for the best thread/block configuration for your GPU...
Best: 224 threads, 96 blocks (3.40282e+38ms)Error: invalid device function
/home/wine/ICPCUDA-master/src/Cuda/pyrdown.cu:90
this error is from:
pyrdown.cu:90:
cudaSafeCall( cudaGetLastError() );
when I cmake it, I got:
-- CUDA NVCC target flags: -gencode;arch=compute_50,code=sm_50;-gencode;arch=compute_52,code=sm_52
-- Configuring done
-- Generating done
-- Build files have been written to: /home/wine/ICPCUDA-master/build
This CUDA arch works well with my ElasticFusion, I don't know how to deal
with this error.
Hi Tom,
I'm comparing the result of ElasticFusion with ICPCUDA pose.
I convert both result to homogeneous transformation matrix, however the results seem different as shown (e.g. in frame no. 5) below:
ElasticFusion:
0.999971 -0.00527336 0.00551413 -0.00348684
0.00529863 0.999976 -0.00454144 -0.00215366
-0.00548995 0.00457042 0.999975 -0.005605
0.0000000000 0.0000000000 0.0000000000 1.0000000000
ICPCUDA:
0.999995 -0.00277173 0.00132549 0.000447368
0.00277131 0.999996 0.000315587 -0.00156628
-0.00132636 -0.000311912 0.999999 -0.000628453
0.0000000000 0.0000000000 0.0000000000 1.0000000000
Are both of them using different coordinate format or other constraint?, is it possible to compare these methods?
note: I used raw depth input instead of using scaled one (as tum rgbd-dataset), the above is example of my own dataset, where works great in ElasticFusion.
Thanks.
Can the sample code be built under VS2017 or VS2015
Hey Tom,
I picked your original code and tried with just 2 depth maps, the transform values I got when processing the first depth map with the second is not nearly the inverse of when processing the second depth map with first. Below is the result:
Transformation output when using depth 0-1.txt:
1305031453.404816 0.00677987 0.00253431 -0.00944612 -0.00188307 0.00523863 0.00920236 0.999942
Transformation output when using depth 1-0.txt:
1305031453.374112 -0.00741758 -0.00220185 0.00926915 0.00199852 -0.0049043 -0.00883814 0.999947 (This should be nearly inverse of above)
I also tried adding First and Second depth maps one after other multiple times, and I can see the drift in the output.
You can check these input maps in depth.txt and results in output.poses.txt The drift can be seen in output at line: 1, 3, 5. Ideally, line 2, 4, 6 should be close to identity.
Do you have any advice regarding this ? and which part of ICP should I look to resolve it ?
I am facing this same drift problem when using it with real-time data.
Hi, I am a bit confused about the transform that was exported to the output.poses file.
Is it a transformation from camera coordinate to world world coordinate?
Hi, and thank you for making this code available. I have it building, but would like to pass two PCL clouds in, and run the icp on those.
for example:
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudQuery(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudRef(new pcl::PointCloud<pcl::PointXYZI>);
if (pcl::io::loadPCDFile<pcl::PointXYZI>("query_114.pcd", *cloudQuery) == -1) //* load the file
{
PCL_ERROR("Couldn't read file query_114.pcd \n");
return (-1);
}
if (pcl::io::loadPCDFile<pcl::PointXYZI>("reference_114.pcd", *cloudRef) == -1) //* load the file
{
PCL_ERROR("Couldn't read file reference_114.pcd \n");
return (-1);
}
Where the clouds are literally just three floats per point, as:
-5.4740019 -4.1216536 8.3391247
What do i need to adjust to pass these clouds to your code?
Thanks again!
Sorry there are a lot of links on the various pages, could you point me to the exact dataset.txt file you're referring to in the README.md?
I get this error:
GeForce GTX 1070
Searching for the best thread/block configuration for your GPU...
Best: 224 threads, 96 blocks (3.40282e+38ms)Error: invalid device function /home/francisco/git/ICPCUDA/src/Cuda/pyrdown.cu:90
I am using Cuda 8.0 and Ubuntu 16.04
May be ICPCUDA doesn't work with Cuda 8.0?
Thanks in advance.
There should be a license text (GPLv3, MIT, BSD..) + license headers in all source files that describes the terms of use. Now it's not even clear if this project is open source or not. Making the code available doesn't make the code legally open source.
I also got a lot of similar errors "error: calling a __host_function("Eigen::Matrix<float, .........") from a __device__function("blockReduceSum<(int)29") is not allowed. My OS is ununtu 14.04, CUDA 8.0, Eigen 3.3.3, do you have any idead how to solve this problem? Thank you!
0.000000 0 0 0 0 0 0 1
0.000000 0 0 0 0 0 0 1
0.000000 0 0 0 0 0 0 1
0.000000 0 0 0 0 0 0 1
Why do I generate all the data that is zero?
thank you
Thanks for the code. I've been testing it, so more to come later. But initially, it crashes at this line
ICP.cpp | 170: icpOdom.getIncrementalTransformation(trans, rot, threads, blocks);
The reason that I found was because the member attribute iteration is not well allocated in the icpOdom's constructor. So I changed the line
ICPOdometry.cpp | 33: iterations.reserve(NUM_PYRS);
into
ICPOdometry.cpp | 33: iterations.resize(NUM_PYRS);
and it works fine till now.
I run this program on RTX 3060 CUDA11.0, but it cannot reach the nominal 750HZ, only 300HZ, how can I solve it?
I have trouble compiling the source. I'm on Ubuntu 14.04, g++ 4.9.4, and using all most recent Pangolin and Eigen from your repository. Below is the error message I got
/home/jjpark7/depth/ICPCUDA/src/ICP.cpp: In function ‘uint64_t loadDepth(pangolin::Image<short unsigned int>&)’: /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:46:138: error: invalid conversion from ‘short unsigned int*’ to ‘std::size_t {aka long unsigned int}’ [-fpermissive] pangolin::Image<unsigned short> depthRaw16((unsigned short*)depthRaw.ptr, depthRaw.w, depthRaw.h, depthRaw.w * sizeof(unsigned short)); ^ In file included from /home/jjpark7/depth/Pangolin/include/pangolin/image/image_io.h:31:0, from /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:6: /home/jjpark7/depth/Pangolin/include/pangolin/image/image.h:44:12: note: initializing argument 1 of ‘pangolin::Image<T>::Image(std::size_t, std::size_t, std::size_t, T*) [with T = short unsigned int; std::size_t = long unsigned int]’ inline Image(size_t w, size_t h, size_t pitch, T* ptr) ^ /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:46:114: error: invalid conversion from ‘std::size_t {aka long unsigned int}’ to ‘short unsigned int*’ [-fpermissive] pangolin::Image<unsigned short> depthRaw16((unsigned short*)depthRaw.ptr, depthRaw.w, depthRaw.h, depthRaw.w * sizeof(unsigned short)); ^ In file included from /home/jjpark7/depth/Pangolin/include/pangolin/image/image_io.h:31:0, from /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:6: /home/jjpark7/depth/Pangolin/include/pangolin/image/image.h:44:12: note: initializing argument 4 of ‘pangolin::Image<T>::Image(std::size_t, std::size_t, std::size_t, T*) [with T = short unsigned int; std::size_t = long unsigned int]’ inline Image(size_t w, size_t h, size_t pitch, T* ptr) ^ /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:60:49: error: no match for call to ‘(pangolin::Image<short unsigned int>) (unsigned int&, unsigned int&)’ depth.RowPtr(i)[j] = depthRaw16(j, i) / 5; ^ /home/jjpark7/depth/ICPCUDA/src/ICP.cpp: In function ‘int main(int, char**)’: /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:111:5: error: ‘ManagedImage’ is not a member of ‘pangolin’ pangolin::ManagedImage<unsigned short> firstData(640, 480); ^ /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:111:28: error: expected primary-expression before ‘unsigned’ pangolin::ManagedImage<unsigned short> firstData(640, 480); ^ /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:112:5: error: ‘ManagedImage’ is not a member of ‘pangolin’ pangolin::ManagedImage<unsigned short> secondData(640, 480); ^ /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:112:28: error: expected primary-expression before ‘unsigned’ pangolin::ManagedImage<unsigned short> secondData(640, 480); ^ /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:114:46: error: ‘firstData’ was not declared in this scope pangolin::Image<unsigned short> firstRaw(firstData.w, firstData.h, firstData.pitch, (unsigned short*)firstData.ptr); ^ /home/jjpark7/depth/ICPCUDA/src/ICP.cpp:115:47: error: ‘secondData’ was not declared in this scope pangolin::Image<unsigned short> secondRaw(secondData.w, secondData.h, secondData.pitch, (unsigned short*)secondData.ptr); ^ make[2]: *** [CMakeFiles/ICP.dir/ICP.cpp.o] Error 1 make[2]: *** Waiting for unfinished jobs.... make[1]: *** [CMakeFiles/ICP.dir/all] Error 2 make: *** [all] Error 2
Hi, thank you fro your great work about ICPCUDA. I want to use this project on my Nvidia NX board. While I am not sure about this method. Can this project run on Nvidia jetson board?
Thank you for your work.
Did anyone encountered error like this?
Error: an illegal memory access was encountered /workspaces/d435i_depth/libs/ICPCUDA/Cuda/containers/device_memory.cu:121
Trying to run on GTX 980ti, compiled on Ubuntu 18.4 with 10.2 Cuda.
I would appreciate any help.
Hi, I am currently experimenting with ICPCUDA.
When I checkout exp Branch, run it with rgbd_dataset_freiburg1_desk and feed my poses to the TUM evalutaion tool (off- or online) I get an error (rmse) from about 0.147 (about the same as stated in readme) and a plot which looks like this:
So far so good.
But when I checkout the master branch, the results are completely off:
compared_pose_pairs 593 pairs
absolute_translational_error.rmse 0.820422 m
absolute_translational_error.mean 0.765179 m
absolute_translational_error.median 0.817489 m
absolute_translational_error.std 0.295962 m
absolute_translational_error.min 0.282498 m
absolute_translational_error.max 1.258876 m
any ideas?
I want to use lidar 3d point clouds data instead of rgbd data, how can I add this support?
Hi, Thank you again for this code. i am attempting to pass cv::mat frames in,instead of using pangolin::Image format. I have this working, but the results are different when i run Mats, vs when I run the pangolin Image as in the example. My code is:
Eigen::Matrix4f IcpCuda::RunIcp(cv::Mat frame1, cv::Mat frame2)
{
cv::Mat mat_ushort1(frame1.cols,frame1.rows, CV_16UC1);
frame1.convertTo(mat_ushort1, CV_16UC1);
cv::Mat mat_ushort2(frame1.cols, frame1.rows, CV_16UC1);
frame2.convertTo(mat_ushort2, CV_16UC1);
unsigned short* dataMat1 = mat_ushort1.ptr<unsigned short>();
unsigned short* dataMat2 = mat_ushort2.ptr<unsigned short>();
icpOdom.initICPModel(dataMat1);
icpOdom.initICP(dataMat2);
.............
The working pangolin code I am using is:
uint64_t loadDepthFromPath(std::string filepath, pangolin::Image<unsigned short> & depth)
{
pangolin::TypedImage depthRaw = pangolin::LoadImage(filepath, pangolin::ImageFileTypePng);
pangolin::Image<unsigned short> depthRaw16((unsigned short*)depthRaw.ptr, depthRaw.w, depthRaw.h, depthRaw.w * sizeof(unsigned short));
for (unsigned int i = 0; i < 480; i++)
{
for (unsigned int j = 0; j < 640; j++)
{
depth.RowPtr(i)[j] = depthRaw16(j, i) / 5;
}
}
uint64_t time = getCurrTime();
return time;
}
I realize this is a little off topic, but if you could help me out here it would be very much appreciated.
results:
//using the Mat frames
GeForce GTX 1080 Ti
ICP: 2.5480ms
ICP speed: 392Hz
0.9998 0.0172 -0.0087 0.0003
-0.0172 0.9999 -0.0010 0.0002
0.0087 0.0012 1.0000 -0.0002
0.0000 0.0000 0.0000 1.0000
//using pangolin images
GeForce GTX 1080 Ti
ICP: 2.7740ms
ICP speed: 360Hz
0.9998 -0.0184 0.0104 0.0068
0.0184 0.9998 0.0039 0.0025
-0.0105 -0.0037 0.9999 -0.0094
0.0000 0.0000 0.0000 1.0000
Thanks!
I did not find any information about the licensing of your code? BSD? Please add it somewhere (usually at the top of each file)
Thanks
Simon
The focal length given in TUM dataset link is fx=525.0,fy=525.0 In the code it is taken as 528, 528. Which one is correct?
Thanks for your code first.
I used the dataset "fr1/xyz" from Computer Vision Grouphttp://vision.in.tum.de/data/datasets/rgbd-dataset/download to test your code, and used the result "slow.poses", depth images and rgb images to make the point cloud as the image "pointcloud_icp.png". I also used the ground truth to make the point cloud as the image "pointcloud_groundtruth.png". The image "1.png" shows the actual scene.
Why the point cloud result of ICP is abnormal?
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.