26 using namespace gtsam;
29 noiseModel::Diagonal::shared_ptr priorNoise =
30 noiseModel::Diagonal::Sigmas(
Vector3(1e-6, 1e-6, 1e-8));
32 graph_.emplace_shared<PriorFactor<Pose2> >(0, Pose2(0, 0, 0), priorNoise);
61 using namespace gtsam;
67 LevenbergMarquardtParams parameters;
70 parameters.relativeErrorTol = 1e-5;
73 parameters.maxIterations = 500;
79 Values result = optimizer.optimize();
81 Values::ConstFiltered<Pose2> viewPose2 = result.filter<Pose2>();
84 for(
const Values::ConstFiltered<Pose2>::KeyValuePair& key_value: viewPose2)
87 karto::Pose2 pose(key_value.value.x(), key_value.value.y(),
88 key_value.value.theta());
90 corrections_.push_back(std::make_pair(key_value.key, pose));
92 graphNodes_.push_back(Eigen::Vector2d(key_value.value.x(),
93 key_value.value.y()));
101 using namespace gtsam;
118 using namespace gtsam;
133 Eigen::Matrix<double,3,3> cov;
135 cov(0,0) = precisionMatrix(0,0);
137 cov(0,1) = cov(1,0) = precisionMatrix(0,1);
139 cov(0,2) = cov(2,0) = precisionMatrix(0,2);
141 cov(1,1) = precisionMatrix(1,1);
143 cov(1,2) = cov(2,1) = precisionMatrix(1,2);
145 cov(2,2) = precisionMatrix(2,2);
147 noiseModel::Gaussian::shared_ptr model = noiseModel::Diagonal::Covariance(cov);
151 graph_.emplace_shared<BetweenFactor<Pose2> >(sourceID, targetID,
155 ROS_DEBUG(
"[gtsam] Adding Edge from node %d to node %d.", sourceID, targetID);