How to add a new factor to graph

[inuex35]

Hello,

Thank you for your excellent work on this project.

I would like to ask how to add new factors to the graph, such as wheel speed or TDCP (Time Differences of Carrier Phase). What parts of the source code should be modified to incorporate these factors?

Thank you.

[chichengcn]

It takes 3 steps to add a new factor to your estimator:

1. Create a new class (factor) to specify residual and Jacobian computaion.

There are many factor implementation in GICI source codes, namely *_error*.cpp and *_error*.h. These factors are located according to its property. If a factor is relevant to a specific sensor, the code will be placed into the sensor folder, for example, the doppler_error factor was placed into the gnss folder. If a factor is commonly used, the code will be placed into the estimate folder, for example, the pose_error factor was placed into the estimate folder.

The wheel speed factor is similar to velocity_error and nhc_error, and the TDCP error is similar to doppler_error and phaserange_error, you can refer to them.

2. Add corresponding functions to sensor base classes to specify adding, erasing, and marginalizing operations.

Currently, we have defined 4 sensor base classes to conduct node operations towards factor graph, including gnss_estimator_base, gnss_loose_estimator_base, imu_estimator_base, and visual_estimator_base.

Taking the velocity_error as example, we have implemented addGnssVelocityResidualBlock, addGnssVelocityResidualMarginBlock, and eraseGnssLooseResidualBlocks to add, marginalize, and erase velocity_error factor to/from graph. You can also implement some optional operations to enrich the operations towards this factor. Such as implementing rejectGnssPositionAndVelocityOutliers to apply outlier rejection for corresponding factors.

3. Inherit the above sensor base classes to your estimator class, and add factors to graph.

Taking the GNSS/INS loosely coupling estimator (gnss_imu_lc_estimator) as example. It inherits gnss_loose_estimator_base and imu_estimator_base, adds INS pre-integration, GNSS position, GNSS velocity, ZUPT, HMC, NHC factors by insertImuState, addGnssPositionResidualBlock, addGnssVelocityResidualBlock, addZUPTResidualBlock, addHMCResidualBlock, addNHCResidualBlock, rejects GNSS outliers by rejectGnssPositionAndVelocityOutliers, and marginalizes out-of-window parameters and residuals by addImuStateMarginBlockWithResiduals.

[inuex35]

Thank you, I will try to implement it.
At least I could build a new factor yesterday(I don't know the implementation is right though)
If I have additional questions, I will post them here.

[inuex35]

Hello,

I just realized you already implemented addRelativeAmbiguityResidualBlock, and I think TDCP can be implemented by modifying this residual. It seems to be based on a similar concept.

What do you think about it?

[chichengcn]

I'm sorry to tell you that this is not the case. The addRelativeAmbiguityResidualBlock factor adds relative constraint between two-epoch ambiguity parameters, i.e. N_k+1 = N_k. It has little correlation with "add relative phase error".

[inuex35]

Hello thank you for the reply.
I got a small progress and will keep trying to implement tdcp.
Additionally let me ask one more question, I want to use multi anntenas and what parts of the source code should be modified including adding the second anntena stream? Mainly I want to know how to add a stream, adding factor is the same with the previous question.
Thank you.

[chichengcn]

I suggest two options. First, finalize the dual-antenna algorithm at outside and feed the attitude solution into gici through the Solution structure. Second, directly input the raw observations through separated streams for each antenna. In detail,

1. Feeding attitude solution.

(1) Encode your attitude data into any stream message format. If you do not have proper message format, you can use our extended NMEA format, see NmeaFormator.

(2) Instantiate a formator from FormatorBase, and implement its decode function. The output of decode should be Solution in DataCluster. Then, add the new formator to enum class FormatorType, MAKE_FORMATOR, MAP_FORMATOR, and void convert(const InType& in, OutType& out).

(3) Handle the Solution data at an estimator. Taking gnss_imu_lc_estimator as an example. Extend addMeasurement to accept the data: if (... measurement.solution_role == SolutionRole::Attitude ...). Note that the role SolutionRole::Attitude corresponds to attitude for xxx_roles in yaml file, you can find or modify its map at the aforementioned template function convert.

(4) Extend addGnssSolutionMeasurementAndState to add attitude factor to graph.

(5) Configure yaml file to connect streamer, the new instantiated formator, and the LC estimator.

2. Feeding raw observations.

(1) The observations from other antennas are the same as the major or reference station antennas. So they can be fed into gici directly by default streamers and formators.

(2) The corresponding xxx_roles in yaml file should be set as heading (we have this option already, see convert, but we did not implement its algorithms). Then the type GnssRole::Heading should be handled at estimators. Since the multi-antenna heading algorithm is similar as the RTK algorithm, we take the rtk_estimator as an example. In its addMeasurement function, the observations are handled by DifferentialMeasurementsAlign, this class stores the observations, and aligns them to make sure the timestamps are corresponding. So the DifferentialMeasurementsAlign class should be extended to align multi-antenna obseravtions.

(3) Re-write addGnssMeasurementAndState to support multi-antenna processing.

(4) If you want to add an new estimator type to handle multi-antenna problem: Extend the EstimatorType, estimatorTypeContains, estimatorTypeToString, and convert. Add initialization codes estimator_.reset(new ...) in multisensor_estimating and multisensor_single_thread_estimating (only for unstable branch).

[inuex35]

Hello,

Thank you very much for all the information you've provided and for your help so far. I apologize for taking more of your time, but I would like to add QZSS to the project as well.

I have been following this approach:

70b9c98

But unfortunately, the program crashes. Could you let me know if there are any related commits or suggest a method to address this issue?

Thank you!

[chichengcn]

I think all the modifications you have done are necessary, but i'm not sure if there are still other codes should be modified. I suggest you to run the code in debug mode to check out why it crashes. Although the debug mode runs very slow because of the Eigen library, but it does not matter if you are using the GNSS-only estimator, since it will not cause crash because of multi-sensor time-alignment issue.