Closed mmp52 closed 4 years ago
Dear @pjrambo and @xuhao1 ,
After debugging in Estimator::relativePose function I have figured out that I have an average parallax of "nan", and then I printed my "corres" vector's size and I saw reasonable values in between 25-100+. Then I have printed out every parallax in the loop, when Estimator::relativePose is calculating the sum_parallax (which is used to calculate average_parallax). I do not know why but while most of the parallax'es in the sum has reasonable values (comparable with the euroc datasets') I have a few of them as "nan"s and that cause all the trouble and result in a nan average. What can be the reason having a "nan" parallax inside the corres? Having these nan result in false return value of the relativePose(),
thank you for your help,
Sincerely Yours, Metin
I have solved the problem finally - I have re-calibrated my camera using a KANNALA-BRANDT model instead of fisheye-mei model. Reading this https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/issues/57 inspired me, I believe there are more people having this problem but there is no answer or suggestion from the original authors. I've done the intrinsic calibration of camera from scratch with kannala-brandt camera-model option, then I have used the resulting new yaml. I was using estimate_extrinsic property of the VINS-Fusion previously (with manually calculated initial guess for body_T_cam0 matrix), I kept doing so. I did not change anything in intrinsics of IMU. Before changing camera model, setting estimate_extrinsic=0 or =1 did not help system work.
Thank you for your comments, which helped me solve the problem.I also had an infinite problem with the results. When I re-calibrated the camera using the kannala-brandt model, I was able to output the pose, but it was not correct.Now, the body_T_cam0 matrix is wrong, what does this matrix mean, the parameters of the imu and the camera?How to calibrate it?
Hey @tianxinyu0509 , sorry for the late reply. In the config folder of VINS-Fusion-gpu, you can find a pdf file named extrinsic_parameter_example.pdf. It gives rotation and translation matrix examples, rotation matrix is a 3x3 matrix composed of sequential rotations, translation matrix is just translation in x,y,z axes. The body_T_cam0 matrix is basically composed of augmenting 3x3 imu_R_cam rotation matrix and transpose of imu_T_cam translation matrix, and adding 0 0 0 1 to the 4th row. So basically; [imu_R_cam(1,1),imu_R_cam(1,2),imu_R_cam(1,3), imu_T_cam(1); imu_R_cam(2,1),imu_R_cam(2,2),imu_R_cam(2,3), imu_T_cam(2); imu_R_cam(3,1),imu_R_cam(3,2),imu_R_cam(3,3), imu_T_cam(3); 0,0,0,1] is the 4x4 opencv transformation matrix. If you want to know what is a rotation matrix, https://en.wikipedia.org/wiki/Rotation_matrix can help as a start, it is basically a matrix which is used to get orientations in one frame with respect to the other. For example, (orientations in imu frame)=imu_R_cam*(orientations in cam frame) . It is actually composed of augmenting unit vectors of one frame expressed in terms of other frames', but I am sure you can find more detailed information about them. You can see from the VINS-Fusion-gpu/config/euroc/euroc_mono_imu_config.yaml that in the config file they have a body_T_cam0 matrix with data: data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, 0, 0, 0, 1]; If you make the augmentation as I have suggested it will give very close values of: [0, -1, 0, -0.02, 1, 0, 0, -0.06, 0, 0, 1, 0.01 0, 0, 0, 1] , which is very close to the real calibrated data. The difference is due to compensating measurement errors by precise calibration. So what did I do to calibrate extrinsic parameters between IMU and Camera? I have calculated the rotation matrix and translation vector using a ruler (very roughly), then I have chosen the estimate_extrinsic:1 option in config file, so my hand-calculated rough values were used as an initial guess to the estimation of the extrinsic parameters, then I have noted resulting values and used them as accurate guesses by setting estimate_extrinsic:0 in following tests. The resulting values coming from estimate_extrinsic:1 case were very close to the values that I've calculated by hand, just as in the case of euroc_mono_imu_config.yaml and extrinsic_parameter_example.pdf.
I hope this helps, Metin
Hey @mmp52. Thanks for your detailed reply, it is a good idea to use the results of vins-fusion optimization as the initial value.I'll give it a try. As I said before, there were inaccurate results, but after I re-calibrated my camera, the results were acceptable. Best wishes to you.
Hello, friend @mmp52 , I have some additional questions about GPU acceleration that I want to discuss with you.Are you running code on TX2 in real time?I tested it on the TX1 and desktop (the graphics card is nvidia GT730) and Shared some data with you. First, the original CPU version on TX1 ran at a frame rate of 8.8FPS, while the GPU version ran at 10.1FPS.I also tested the functions in feature_tracker.cpp.CPU version of cv: : goodFeaturesToTrack is 50 FPS (used here PFS is not accurate, means the number of code runs per second), the GPU version of the cv: : cuda: : createGoodFeaturesToTrackDetector is 62 FPS.CPU version of cv: : calcOpticalFlowPyrLK is 54 FPS, the GPU version of the cv: : cuda: : SparsePyrLKOpticalFLow: : crate is 51 FPS.I'm confused. On the TX1, the GPU is no better than the CPU.What are you like on TX2?
I also tested it on the desktop (i5-7300,GT730), and the results are as follows: cpu The corresponding GPU version VINS 25.3FPS 25.7PFS goodFeaturesToTrack 256FPS 261FPS calcOpticalFlowPyrLK 351FPS 247FPS This data means that the GPU version is not being accelerated.But there are a few questions worth asking:
Sorry, there is something wrong with the layout of the table.
Hello dear @tianxinyu0509 , I have not succeeded to make it work on TX2 in real time, I am still trying to do so, but I completely believe that CPU capacity limits the potential of TX2 - which means GPU acceleration does not accelerate much. As soon as I make it work on my new setup (I have rebuilt everything on a new xavier) I will write down to you. Meanwhile if you find a way to make it faster please let me know :) Also, I've read that usb_cam node of the ROS depends highly on CPU, maybe that is the reason. If I cannot make it work real time on 25-30FPS at Xavier (which is way more capable than tegra) I will try to change camera node to something else. Which method do you use to get the image? Are you also using usb_cam node of ROS or something else?
Dear friend @mmp52 , my thoughts are as follows and I hope to be of some help. You can use the following commands to turn on the TX2 and Xavier high performance modes. cd /home/nvidia sudo nvpmodel -m 0 sudo ./jetson_clocks.sh After open high-performance model, I can reach 15 FPS on TX1,on TX2 and Xavier should be better.You can see the relevant information of Xavier jetson_clocks. Sh I don't know exactly what usb_cam node of the ROS is, I haven't seen it.My camera is indemind.The manufacturer provides some SDKS, on which I add ROS communication. If you want to make sure whether the usb_cam node problem, I think you should try the data set, if the data set could run high FPS, and the actual camera doesn't, may be the reason.
Thank you very much @tianxinyu0509 , one more question: Do you calibrate your IMU? Do you offset gravitational acceleration to 0 when robot is not moving? Thank you for your previous answer!
@mmp52 I'm going to do it, and if there's any progress, I'll share it here.Thank you very much.
Hello, friend. I have a result to tell you.I found a big pitfall: the GPU version of VINS behaves differently than the CPU version.I recorded a dataset with a camera and tested it with two different versions of VINS, and found that the GPU version's trajectory performed worse.Reasons should be CV: : cuda: : createGoodFeaturesToTrackDetector and CV: : goodFeaturesToTrack result is different.The results are disappointing: we get worse results using gpu.
@mmp52, I'm sorry that I haven't replied to your question about IMU for a long time. I've only recently finished adding an imu. There are some things to note here, but the first thing to note is the units of the imu, the imu data that goes into the code has to be in international units: g/m2,rad/s. Second, imu will output 9.8m/s2 when it is stationary. There's a function in the code that deals with gravity. In addition, I found that the body_T_cam matrix affects the accuracy of the results. I have chosen the estimate_extrinsic:1 option in config file. The optimized data is then used as the initial value and also chose the estimate_extrinsic:1. Repeat this, and I find the results getting worse and worse. An accurate body_T_cam matrix cannot be obtained through VINS optimization, and may have to be calibrated with kalibr.
@tianxinyu0509 , Thank you for your answers friend. After a few iterations of estimating extrinsic parameters (body_T_cam) I have decided on a fixed value matrix, I give that one as an input to the system each time with estimate_extrinsic:1 and it gives similar results each time. Maybe you have a wrong axis configuration, it took me long time to find correct transformation matrix because I was wrong on IMU axis, I have confused +/- values on IMU. Anyway, it works somehow but now I am trying to improve the performance by configuring correct noise inputs on IMU. I have another question on that topic, if you are interested you can help me :) IMU noise values By the way, does kalibr help with intrinsic IMU parameters? Again thank you for your detailed answers, good luck on your project.
@mmp52 ,Thank you for your reply.I can make sure that my imu axis is correct.The optimized body_T_cam matrix, do you add "printf” or "cout" in the code to show it?
In the parameters file, there is a path that also prints the body_T_cam matrix:
output_path: "/home/tong/output/"
After my test, this path will output the matrix about 10S after running, which I think is not very accurate.
@tianxinyu0509 I have directly used/copied the comments on the thread in which I run "vins_node", VINS automatically prints you the required correction matrix , if that is pretty close to the identity matrix that means it did not change much when it is esimating extrinsic values when it started from your iteration. I have previously used cout to print matrices but I remember it requires iteration of elements. Also, as you've written VINS writes down resulting matrix to output folder, I believe that can also be used, also commented matrix on the thread happens more or less 10 seconds after start.
@tianxinyu0509 Hello friend, do you know how can I check if there is a lag in the algorithm when I run it on real time? also did you solve the problem of transformation matrix ? I have calibrated tf and imu values with kalibr and imu_utils and it is working well now, but I do not know if the values are lagging or not
@mmp52 In the code rosnodetest.cpp, the author uses the buffer :img0_buf,imu_buf to store the received image data and imu data.So low CPU performance can lead to algorithms that are not real-time.You can play a .bag dataset.If the data set is finished and the algorithm is still running, then the delay can be determined. If only use the camera, I replaced the buffer with a variable, or set the maximum number of buffers to 1.In this case, the buffer will only store the latest data received. I haven't tried to see if this is possible with the imu.Because data is synchronized between the imu and the image, this approach can lose a lot of data if CPU performance is poor.
@tianxinyu0509 Thank you for your answer
Hello,
I am having the error "Not enough features or parallax; Move device around" when running VINS Mono+IMU on tx2. I have a 1280*720 camera. I have an initial guess of the transformation matrix, even though I try the algorithm with estimate_extrinsic = 0 , 1 or 2 none of them works. I have tried to increase the feature number to 400, it also did not work. Moving the device was also useless for me. I have checked the IMU signal and visualized camera track, camera seems to detect the features well and IMU is working at around 50 hz, increasing it up to 80 is also useless. This message is sent to ROS_INFO @566th line of the estimator.cpp of the code, I got that the problem is with initialization process but I cannot figure out how to solve it.
Also I have realized that in your rosNodeTest.cpp code, feature_callback function feeds an estimator object with estimator.inputFeature(t,feature) but /feature_tracker/feature topic has no advertiser, I don't think this is the cause of the problem because this topic was also void when I ran euroc examples.
Finally, is just publishin IMU with linear accelerations and angular velocity enough? I could not figure out that should I pre integrate imu and publish it in another topic just for initalization process or not.
Thanks for your help Metin