Working on porting code from MATLAB to C++
Cite: Ma, Qianli, Zachariah Goh, Sipu Ruan, and Gregory S. Chirikjian. "Probabilistic approaches to the AXB= YCZ AXB= YCZ calibration problem in multi-robot systems." Autonomous Robots 42 (2018): 1497-1520. https://doi.org/10.1007/s10514-018-9744-3