question: can I specify edges in posegraph3d/factorgraph with the same unknown value?

In the considered problem there is an uncertain transformation between the lidar and the GPS device (more or less known, calibrated but not the best). Could I add the uncertain edge between the GPS and lidar nodes (for every measurements) such that the optimisation will look up for a common value? (Maybe with adding uncertainty that can be different for each measurements.)

If the current MATLAB syntax does not consider this feature, the original GTSAM c++ repository would allow? (Then I do the wrapping as a mex....)

Thx, in advance, I like the toolbox in general, but this field is quite new for me...

Answer 1

[Akshai Manchana]

Use factorGPS and factorTwoPoseSE3 to construct a factor graph optimization problem in the following way.

For example consider the following sensor fusion scenario. Two syncronized sensors GPS and 3D-LIDAR are acquiring data simultaneously.

• gpsReadind1 - [latitude longitude altitude] at first GPS sampling instant
• gpsReading2 - [latitude longitude altitude] at second GPS sampling instant
• relPose ([x,y,z,qw,qx,qy,qz]) - Relative pose between frist and second GPS sampling instants computed using 3-D lidar scan alignment. Imagin that there are 2 lidar scans acquired at exactly GPS sampling instants.
• gpsToLidarTransform - A SE3 ([x,y,z,qw,qx,qy,qz]) transformation from GPS frame to LIDAR sensor frame. This may not be accurately known as specified in the question. A rough trasnform is expected to initialize the optimization problem better than completely unknown quantity.

% Each sensor pose may be considered as a node in the factor graph. The
% nodes in the factor graph will be optimized to satisfy constraints from
% multiple sensors like GPS and Lidar. Each node in the graph is
% represented by a unique ID.
firstPoseID = 1;
secondPoseID = 2;

% Relative pose between first and second lidar/GPS sampling instants
relPose = [1,0,0,1,0,0,0]; % observe identity quaternion and only X motion
% Construct a TWO Pose factor to add a LIDAR relative pose constraint to
% graph
lidarFactor = factorTwoPoseSE3([firstPoseID,secondPoseID],Measurement=relPose);

% Construct 2 GPS factors to add a GPS measurement constraint to factor
% graph.
gpsReading1 = [5 5 1000]; % (latitude longitude altitude). Latitude and longitude are in degrees and altitude is in meters.
gpsReading2 = [5,5,1001]; % Imagine that there's only altitude change or Z-direction movement
hdop = 1.5; % Use your GPS sensor Horizontal dilution of precision
vdop = 2.4; % Use your GPS sensor Vertical dilution of precision
% consider the first GPS reading as the origin of local coordinate system
% of GPS
gpsFactor1 = factorGPS(firstPoseID,Location=gpsReading1,HDOP=hdop,VDOP=vdop,ReferenceLocation=gpsReading1,ReferenceFrame="NED");
gpsFactor2 = factorGPS(secondPoseID,Location=gpsReading2,HDOP=hdop,VDOP=vdop,ReferenceLocation=gpsReading1,ReferenceFrame="NED");

% True Z-motion in GPS frame translates to true X-motion in Lidar frame. 
% i.e the true transform from lidar to GPS frame is
% se3([0,pi/2,0],"eul","ZYX"). But let's consider an inaccurate transform
% and let the factor graph get to the best possible estimate.
actualGPSToLidarTransform = se3([0,pi/2,0],"eul","ZYX");
gpsToLidarTransform = se3([0.05,(pi/2 - 0.05),0.05],"eul","ZYX");

% first add the lidar factor to graph and consider the pose nodes in the
% graph represent lidar poses. Usually this is done because lidar is the
% faster sensor among the 2 considered.
fg = factorGraph;
addFactor(fg, lidarFactor);

% add gps factors now. But understand that GPS can only be helpful to
% constrain the location part of the pose and not the orientation.
addFactor(fg, gpsFactor1);
addFactor(fg, gpsFactor2);

% adding the factors won't initialize the pose node values. They need to be
% properly initialize the for the factor graph optimization to converge to
% a global minima instead of a wrong local minima.
lidarPose1 = gpsToLidarTransform.xyzquat; % first lidar pose should be able to convert GPS readings to lidar frame
nodeState(fg, firstPoseID, lidarPose1); % set initial guess

% Optimize the factor graph to refine all lidar poses satifying both lidar
% and GPS constraints and achieve sensor fusion.
opts = factorGraphSolverOptions("VerbosityLevel",2,"TrustRegionStrategyType",0);
optimize(fg);

% retrieve optimized lidar poses
lidarPosesOptimized = nodeState(fg, [firstPoseID,secondPoseID]);
% first lidar pose can transform the locations from GPS to lidar frame.
refinedGPSToLidarTransform = se3(lidarPosesOptimized,"xyzquat");

% find the difference against the actual transform
d = dist(actualGPSToLidarTransform,refinedGPSToLidarTransform);
disp("The difference between refined and actual GPS to LIDAR transform: " + d);

    "The difference between refined and actual GPS to LIDAR transform: 0.00061132"    "The difference between refined and actual GPS to LIDAR transform: 1"

Note that the same optimization problem can be extended to multiple sensor readings. Usually the GPS and LIDAR data will be unsynchronized. Factor graph expects the data to be synchronized. This may require an extra "retime" step to interpolate GPS measurements/readings at exactly lidar sampling time stamps.

Observe that there is no explicit relative pose factor between GPS and LIDAR pose nodes. Only LIDAR pose nodes are represented in the graph and GPS constraints are added to the same LIDAR pose nodes. Indirectly when the graph is optimized the transformation from one sensor to the other is estimated. This can also be achived by connecting LIDAR and GPS nodes at same sampling instants with a factorTwoPoseSE3. The above mentioned formulation is usually used in sensor fusion workflows. More formulations can also be tried like this to find out whichever best suits your workflow.