SfM + Wheeld Robot's Odometry: scene resonstruction issues

C++
, ,
1

Hello.

My goal is to have perform structure from motion via a wheeled robot equipped with a monocular camera.

For now, the robot’s simulated in IsaacSim, the image, camera info, as well as odometry are sent through ROS to my main processing class.

However, I seem to be having an issue where the triangulated points don’t give me a good reconstruction.

Note:

  • I use raw* odometry pose (x,y,z) and orientation (ROS publishes a quaternion which I turn with Eigen’s .toRotationMatrix()) as my camera’s R and t matrices. My understanding is that if the robot starts at t=[0,0,0] and moves 1 meter forward (after reorientation t’=[0,0,1]), then that already suffices storing extrinsic parameters “in relation” to the 1st frame.
  • The room is of size ~7.5*4.5 meters, with the robot starting in the middle. It looks towards a wall, and starts moving forward, tracking keypoints (detected with SURF, tracked with OF) on the wall.
  • Odometry is obtained in meters. The robot moves slowly, with differences in poses being ~a couple of centimeters. I check the p-norm between two consecutive captures, disregarding movement <0.25 meters and rotation <5 degrees).

*IsaacSim uses a [X-forward, Y-left, Z-up] convention. Because of that I use a matrix to reorient the axes:

                | 0  -1   0 | 
worldToOpenCV = | 0   0  -1 |
                | 1   0   0 |

However, the resulting triangulated point cloud (through cv::sfm::triangulatePoints(points2d, projectionMatrices, points3d) appear somewhat warped.

I have consulted: SFM - Estimated 3D points from reconstruction are obscenely high, from which I decided to copy the normalization approach (mean subtracted, scaled uniformly to have unity stddev). This seemed to eliminate the points being very far away, but still results in a “slanted” point cloud.

What could be causing that? Is it an issue related to assumed units, or maybe how I calculate camera pose/orientation? I’m quite lost on this one, if someone could shed a light onto possible issues I’d be very thankful. If there’s any additional information I could provide, please ask.

Thank you for reading.

…
Data from a test with ~600 keypoints found:Camera information:

  • 1st image - left: showcase of the raw sent image right: tracked keypoints
  • 2nd image - resulting point cloud

Screenshot_20251028_203633
Screenshot_20251028_2036331348×1339 128 KB

Intrinsic matrix K (shared):
[ 1301 0 630 ]
[ 0 1302 380 ]
[ 0 0 1 ]

Coordinates of first 50 points after triangulation and normalization:

[depther-2] [INFO] [1761680674.580015271] [depth_node]: Point #0 coords: (-0.007, 0.066, 0.107)
[depther-2] [INFO] [1761680674.580116420] [depth_node]: Point #1 coords: (-0.019, 0.038, 0.080)
[depther-2] [INFO] [1761680674.580131801] [depth_node]: Point #2 coords: (-0.049, -0.006, 0.063)
[depther-2] [INFO] [1761680674.580150407] [depth_node]: Point #3 coords: (-0.051, -0.007, 0.061)
[depther-2] [INFO] [1761680674.580168041] [depth_node]: Point #4 coords: (-0.043, 0.003, 0.069)
[depther-2] [INFO] [1761680674.580184554] [depth_node]: Point #5 coords: (-0.134, -0.128, -0.101)
[depther-2] [INFO] [1761680674.580194057] [depth_node]: Point #6 coords: (-0.042, 0.005, 0.069)
[depther-2] [INFO] [1761680674.580204501] [depth_node]: Point #7 coords: (-0.052, -0.005, 0.064)
[depther-2] [INFO] [1761680674.580219662] [depth_node]: Point #8 coords: (-0.055, -0.015, 0.052)
[depther-2] [INFO] [1761680674.580237677] [depth_node]: Point #9 coords: (-0.033, 0.013, 0.078)
[depther-2] [INFO] [1761680674.580254099] [depth_node]: Point #10 coords: (-0.081, -0.062, 0.003)
[depther-2] [INFO] [1761680674.580263352] [depth_node]: Point #11 coords: (-0.049, -0.010, 0.058)
[depther-2] [INFO] [1761680674.580272485] [depth_node]: Point #12 coords: (-0.010, 0.040, 0.108)
[depther-2] [INFO] [1761680674.580281026] [depth_node]: Point #13 coords: (-0.141, -0.142, -0.113)
[depther-2] [INFO] [1761680674.580289588] [depth_node]: Point #14 coords: (-0.041, -0.003, 0.039)
[depther-2] [INFO] [1761680674.580298300] [depth_node]: Point #15 coords: (-0.052, -0.012, 0.056)
[depther-2] [INFO] [1761680674.580306882] [depth_node]: Point #16 coords: (-0.137, -0.138, -0.110)
[depther-2] [INFO] [1761680674.580317426] [depth_node]: Point #17 coords: (0.554, 0.593, 0.799)
[depther-2] [INFO] [1761680674.580325868] [depth_node]: Point #18 coords: (-0.080, -0.068, 0.006)
[depther-2] [INFO] [1761680674.580334510] [depth_node]: Point #19 coords: (-0.042, -0.004, 0.037)
[depther-2] [INFO] [1761680674.580343022] [depth_node]: Point #20 coords: (-0.009, 0.058, 0.121)
[depther-2] [INFO] [1761680674.580351784] [depth_node]: Point #21 coords: (0.288, 0.456, 0.606)
[depther-2] [INFO] [1761680674.580360245] [depth_node]: Point #22 coords: (-0.051, -0.015, 0.053)
[depther-2] [INFO] [1761680674.580368577] [depth_node]: Point #23 coords: (-0.312, -0.278, -0.230)
[depther-2] [INFO] [1761680674.580378310] [depth_node]: Point #24 coords: (-0.044, -0.000, 0.073)
[depther-2] [INFO] [1761680674.580394562] [depth_node]: Point #25 coords: (0.217, 0.378, 0.429)
[depther-2] [INFO] [1761680674.580410234] [depth_node]: Point #26 coords: (-0.106, -0.085, -0.070)
[depther-2] [INFO] [1761680674.580427017] [depth_node]: Point #27 coords: (0.687, 0.997, 1.150)
[depther-2] [INFO] [1761680674.580445142] [depth_node]: Point #28 coords: (-0.350, -0.428, -0.415)
[depther-2] [INFO] [1761680674.580457088] [depth_node]: Point #29 coords: (-0.045, -0.001, 0.072)
[depther-2] [INFO] [1761680674.580465940] [depth_node]: Point #30 coords: (0.099, 0.205, 0.215)
[depther-2] [INFO] [1761680674.580476415] [depth_node]: Point #31 coords: (-0.153, -0.149, -0.129)
[depther-2] [INFO] [1761680674.580492567] [depth_node]: Point #32 coords: (-0.116, -0.102, -0.087)
[depther-2] [INFO] [1761680674.580557236] [depth_node]: Point #33 coords: (-0.049, -0.001, 0.013)
[depther-2] [INFO] [1761680674.580581439] [depth_node]: Point #34 coords: (-0.076, -0.047, -0.032)
[depther-2] [INFO] [1761680674.580593676] [depth_node]: Point #35 coords: (-0.088, -0.025, 0.012)
[depther-2] [INFO] [1761680674.580602478] [depth_node]: Point #36 coords: (-0.146, -0.135, -0.140)
[depther-2] [INFO] [1761680674.580611170] [depth_node]: Point #37 coords: (-0.208, -0.211, -0.196)
[depther-2] [INFO] [1761680674.580619642] [depth_node]: Point #38 coords: (-0.021, 0.040, 0.050)
[depther-2] [INFO] [1761680674.580628584] [depth_node]: Point #39 coords: (-0.145, -0.135, -0.140)
[depther-2] [INFO] [1761680674.580636976] [depth_node]: Point #40 coords: (-0.199, -0.202, -0.188)
[depther-2] [INFO] [1761680674.580645197] [depth_node]: Point #41 coords: (-0.007, 0.055, 0.099)
[depther-2] [INFO] [1761680674.580653839] [depth_node]: Point #42 coords: (-0.023, -0.050, -0.053)
[depther-2] [INFO] [1761680674.580662320] [depth_node]: Point #43 coords: (-0.052, -0.013, 0.029)
[depther-2] [INFO] [1761680674.580670912] [depth_node]: Point #44 coords: (-0.156, -0.148, -0.149)
[depther-2] [INFO] [1761680674.580679624] [depth_node]: Point #45 coords: (-0.015, 0.043, 0.096)
[depther-2] [INFO] [1761680674.580688046] [depth_node]: Point #46 coords: (-0.001, 0.063, 0.112)
[depther-2] [INFO] [1761680674.580696988] [depth_node]: Point #47 coords: (-0.150, -0.150, -0.146)
[depther-2] [INFO] [1761680674.580705309] [depth_node]: Point #48 coords: (-0.152, -0.143, -0.145)
[depther-2] [INFO] [1761680674.580713631] [depth_node]: Point #49 coords: (-0.007, 0.056, 0.103)
[depther-2] [INFO] [1761680674.580722233] [depth_node]: Point #50 coords: (-0.150, -0.149, -0.146)