Thursday, October 4, 2012

Photoconsistency Visual Odometry II

I have been working on new features and modifications for my Photoconsistency Visual Odometry project. In the last version I have implemented two more C++ classes to estimate the 3D rigid transformation between two RGBD frames. In these new classes, the residuals and jacobians are computed analytically, significantly improving performance. 


Top: resulting 3D map using the new Photoconsistency Visual Odometry implementation. Bottom: visualization of the estimated trajectory over the ground-truth using the CVPR tools.
The changes do not end up there; now the source code is organized in two different parts: the phovo library, which contains the Photoconsistency Visual Odometry algorithms; and applications that use this library. This way you can select to just build the phovo library (which only depends on OpenCV, Eigen and OpenMP), or configure the project to compile the provided applications too. Furthermore, I have implemented two new classes to access the Kinect sensor data for online operation.

If you want to give it a try, please download the latest code from http://code.google.com/p/photoconsistency-visual-odometry/ and build your own 3D maps with your Kinect sensor. 




15 comments:

  1. Hello Miguel,
    Your project is really nice!! I am very interested in your project. I would like to understand how you do that, so I am trying to trace your source code. But it makes me a little confused. Could you provide some document or papers for your nice project? :)

    Best regards,
    John

    ReplyDelete
    Replies
    1. Hello John,

      First of all thank you for your interest! Sure I can help you understand how this work. Which part do you think it is confusing? I have not written any specific document on this, however this solution is inspired by the following two papers and the underlying idea is pretty similar:

      http://vision.in.tum.de/_media/spezial/bib/steinbruecker_sturm_cremers_iccv11.pdf
      http://www.i3s.unice.fr/~comport/publications/2011_ACRA_audras.pdf

      Best regards,
      Miguel Algaba.

      Delete
    2. Hello Miguel,
      Thanks for your kind. I was confused about computeResidualsAndJacobians() in CPhotoconsistencyOdometryAnalytic.

      1.How did you calculate the "temp1" ~ "temp26" ? and what are they meaning?

      2.What is "jacobianPrRt" doing ? Is it related to the gradient of the image?

      Thanks in advances for providing the paper. :)

      Your sincerely,
      John

      Delete
    3. Hi again John,

      I will answer your second question first :). Basically jacobianPrRt is the jacobian of the composition of two functions: projectiveFunction(rigidTransformationFuction).

      - projectiveFunction: This function projects a 3D point to the 2D plane using camera parameters.
      - rigidTransformationFunction: This function applies a 6DoF rigid body motion to a 3D point.

      You can see jacobianPrRt as jacobianPr*jacobianRt where:

      - jacobianPr: This is the first order derivative of the projectiveFunction with respect to (x',y',z'). Where (x',y',z') are the 3D coordinates of a transformed (x,y,z) point.
      - jacobianRt: This is the first order derivative of the rigidTransformationFunction with respect to (x,y,z,yaw,pitch,roll).

      Ok, now I will answer to your first question. As you can imagine, the analytic form of the jacobian contains a lot of operations which are the same for all the 3D points. To avoid computing all those operations for every 3D point I have defined several temporal variables that help me obtain better runtime performance.

      I hope this helps, If you have any other question, please don't hesitate asking :)

      Best regards,
      Miguel Algaba.

      Delete
    4. Hi Miguel,

      Thanks for your answer that is really helpful. But after calculating the jacobian of projectiveFunction and rigidTransformationFunction, the eq. are different with yours. Let me make sure that

      projectiveFunction : Intrinsic Camera Matrix (3x3)
      (focal length and center of x,y)
      rigidTransFunction : Extrinsic Camera Matrix ([R|t] 3x4)
      (x,y,z,yaw,pitch,roll)

      Then I calculate the jacobian of both and multiply them to get jacobianPrRt (2x6). Is that anything wrong or I get the wrong way?

      Your sincerely,
      John

      Delete
    5. Hi again John,

      As you say, jacobianPrRt's size is 2x6 and should be the same if you compute both jacobians and multiply them. Please take a look inside the CPhotoconsistencyOdometryBiObjective class. Within the CPhotoconsistencyOdometryBiObjective.h file, I calculated the jacobianProy and jacobianRt separately. Do you get different equations there too? Can you write me with your equations?

      Best regards,
      Miguel Algaba.

      Delete
    6. Hi again Miguel,
      After calculating jacobianProy and jacobianRt separately, I get the same equations as yours in CPhotoconsistencyOdometryBiObjective class. And I found that I forgot to transform the 3D points by rigid motion for projectiveFunction that is why I got the different equations. And I think the "temp11" should be cos(pitch)*cos(yaw), not cos(pitch)*cos(yaw)+x, in CPhotoconsistencyOdometryAnalytic class. When you calculate the transformed point, it will become px*(cos(pitch)*cos(yaw)+x). It should be px*(cos(pitch)*cos(yaw))+x. But I am not sure it is right.

      Thanks for your hints again :)

      Your sincerely,
      John

      Delete
  2. Hello Miguel

    You did great job. This project really works for me.
    I'm running your program with ASUS X-tion pro, but the result is not good. Then I go into the source code and found that cameraMatrix could be the problem (Because you are running with Kinect).

    So, I calibrate X-tion pro and get intrinsics matrix, then replace cameraMatrix.

    Eigen::Matrix3f cameraMatrix; cameraMatrix <<
    5.4346348954367033e+02, 0., 3.1950000000000000e+02,
    0., 5.4177605601085429e+02, 2.3950000000000000e+02,
    0., 0., 1.;

    Everything works fine and result is good now

    Thank you

    Steve

    ReplyDelete
  3. Hi Steve,

    First of all I apologize for my very late reply, I moved to another city and I had no time to answer before. It is very good to see that you found this project useful and that it worked for the X-tion pro camera with that minimal change.

    Best regards!
    Miguel Algaba.

    ReplyDelete
  4. Hi Miguel. your project is very interesting, very nice work! I'm trying to extend it, by adding the loop closure detection, as you have done in another project (kinestslam6d). I'm using the very same graph optimization library(g2o), but i cannot figure out how to set the information matrix between 2 related poses in the graph(basically the edge). As a reference, I have the paper "Large-Scale Multi-Resolution Surface Reconstruction from RGB-D Sequences", which is the evolution of one of your references, but I've found that for some reason their proposal is not indeed working (the edges in the graph seem not to be weighted correctly by using the (J^t J)^-1).
    Do you have any suggestion? Did you face the same problem? Are you planning to add the graph optimization also to this fantastic project?

    Thank you very much.
    regards,
    geremy

    ReplyDelete
    Replies
    1. Hi Geremy,

      I'm glad to know that you find this project useful. If you want to extend this project to add loop closure, we can collaborate together to make it possible. This is one of the things that I had in mind for a long time, so it would be nice to chat with you about possible solutions. I have some ideas on how to set the information matrix between two poses since I did this same thing for the kinectslam6d project as you mention. For that project I used one "heuristic" value to encode the edge weights based on the number of matched points, but for me it seems better to use the photometric residues for that.

      Please, send me an email so that we can schedule a chat to talk about this.

      BTW, I have pasted here the new repository for the project. The old one in Google Code is not maintained anymore and the new one hosted on Github:
      https://github.com/MiguelAlgaba/photoconsistency-visual-odometry

      I have made some important changes in the new repo and I will continue maintaining it little by little :)

      Best regards,
      Miguel Algaba

      Delete
  5. This comment has been removed by the author.

    ReplyDelete
  6. Hi Miguel,

    I am also implenting visual Odometry, and i am using SURF features to compute the 3D rigid body transformation of the camera. I have implented it :) .

    But it produces bad results when it misses the features. My question is, is there any way to preprocess the image before i do the SURF on it so that i get same SURF features every time. And what to do when we have no features.

    How essential is TRACKING in this context.

    There is a lot to answer, but i would be glad if you answer.


    Thanks. Good Job.
    Junaid

    ReplyDelete
    Replies
    1. Hi Junaid, sorry for my late reply. If you want to track a set of feature points across multiple images you can try to use a similar approach to the one of Kanade-Lucas algorithm. Basically what you can do is to extract the feature points on one image and then track those same points over the next images until you loose the track for each point. This approach will be faster than matching points on each pair of consecutive images and will also produce larger baselines for computing the relative rigid body motion, so you will probably improve the results.

      Best,

      Delete
  7. Sorry i forgot i am also using KINECT sensor to get the 3D information.

    ReplyDelete