Open inuex35 opened 1 month ago
It takes 3 steps to add a new factor to your estimator:
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.
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.
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
.
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.
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?
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".
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.
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) 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.
(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).
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.