33 std::cout << (
s.empty() ?
"" :
s +
" ") << std::endl;
34 std::cout <<
"size: " <<
size() << std::endl;
37 ss <<
"factor " <<
i <<
": ";
40 std::cout << std::endl;
49 const std::function<
bool(
const Factor* ,
double ,
50 size_t )>& printCondition)
const {
51 std::cout <<
str <<
"size: " <<
size() << std::endl << std::endl;
57 std::cout <<
"Factor " <<
i <<
": ";
60 ss.str(std::string());
62 if (
auto mf = std::dynamic_pointer_cast<MixtureFactor>(factor)) {
63 if (factor ==
nullptr) {
64 std::cout <<
"nullptr"
67 factor->print(
ss.str(), keyFormatter);
68 std::cout <<
"error = ";
69 mf->errorTree(
values.nonlinear()).print(
"", keyFormatter);
70 std::cout << std::endl;
73 std::dynamic_pointer_cast<GaussianMixtureFactor>(factor)) {
74 if (factor ==
nullptr) {
75 std::cout <<
"nullptr"
78 factor->print(
ss.str(), keyFormatter);
79 std::cout <<
"error = ";
80 gmf->errorTree(
values.continuous()).print(
"", keyFormatter);
81 std::cout << std::endl;
83 }
else if (
auto gm = std::dynamic_pointer_cast<GaussianMixture>(factor)) {
84 if (factor ==
nullptr) {
85 std::cout <<
"nullptr"
88 factor->print(
ss.str(), keyFormatter);
89 std::cout <<
"error = ";
90 gm->errorTree(
values.continuous()).print(
"", keyFormatter);
91 std::cout << std::endl;
93 }
else if (
auto nf = std::dynamic_pointer_cast<NonlinearFactor>(factor)) {
94 const double errorValue = (factor !=
nullptr ? nf->error(
values) : .0);
95 if (!printCondition(factor.get(), errorValue,
i))
98 if (factor ==
nullptr) {
99 std::cout <<
"nullptr"
102 factor->print(
ss.str(), keyFormatter);
103 std::cout <<
"error = " << errorValue <<
"\n";
105 }
else if (
auto gf = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
106 const double errorValue = (factor !=
nullptr ? gf->error(
values) : .0);
107 if (!printCondition(factor.get(), errorValue,
i))
110 if (factor ==
nullptr) {
111 std::cout <<
"nullptr"
114 factor->print(
ss.str(), keyFormatter);
115 std::cout <<
"error = " << errorValue <<
"\n";
117 }
else if (
auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
118 if (factor ==
nullptr) {
119 std::cout <<
"nullptr"
122 factor->print(
ss.str(), keyFormatter);
123 std::cout <<
"error = ";
124 df->errorTree().print(
"", keyFormatter);
125 std::cout << std::endl;
139 const Values& continuousValues)
const {
140 using std::dynamic_pointer_cast;
143 auto linearFG = std::make_shared<HybridGaussianFactorGraph>();
145 linearFG->reserve(
size());
154 if (
auto mf = dynamic_pointer_cast<MixtureFactor>(
f)) {
156 mf->linearize(continuousValues);
157 linearFG->push_back(gmf);
158 }
else if (
auto nlf = dynamic_pointer_cast<NonlinearFactor>(
f)) {
160 linearFG->push_back(gf);
161 }
else if (dynamic_pointer_cast<DiscreteFactor>(
f)) {
163 linearFG->push_back(
f);
164 }
else if (
auto gmf = dynamic_pointer_cast<GaussianMixtureFactor>(
f)) {
165 linearFG->push_back(gmf);
166 }
else if (
auto gm = dynamic_pointer_cast<GaussianMixture>(
f)) {
167 linearFG->push_back(gm);
168 }
else if (dynamic_pointer_cast<GaussianFactor>(
f)) {
169 linearFG->push_back(
f);
172 throw std::invalid_argument(
173 std::string(
"HybridNonlinearFactorGraph::linearize: factor type "