28 typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> >
NodeVector;
64 ROS_INFO(
"Loop Closure Solve time: %f seconds",
70 karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
71 corrections.push_back(std::make_pair(iter->nodeId, pose));
96 Eigen::Matrix<double,3,3> m;
97 m(0,0) = precisionMatrix(0,0);
98 m(0,1) = m(1,0) = precisionMatrix(0,1);
99 m(0,2) = m(2,0) = precisionMatrix(0,2);
100 m(1,1) = precisionMatrix(1,1);
101 m(1,2) = m(2,1) = precisionMatrix(1,2);
102 m(2,2) = precisionMatrix(2,2);
111 std::vector<float> raw_graph;
114 g.reserve(raw_graph.size()/4);
116 Eigen::Vector2d pose;
117 for (
size_t i=0; i!=raw_graph.size()/4; i++)
119 pose(0) = raw_graph[4*i];
120 pose(1) = raw_graph[4*i+1];
std::vector< sba::Node2d, Eigen::aligned_allocator< sba::Node2d > > NodeVector
int doSPA(int niter, double sLambda=1.0e-4, int useCSparse=SBA_SPARSE_CHOLESKY, double initTol=1.0e-8, int CGiters=50)
void getGraph(std::vector< float > &graph)
const Pose2 & GetCorrectedPose() const
std::vector< Node2d, Eigen::aligned_allocator< Node2d > > getNodes()
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
const Matrix3 & GetCovariance()
virtual std::unordered_map< int, Eigen::Vector3d > * getGraph()
Vertex< T > * GetTarget() const
#define forEach(listtype, list)
kt_int32s GetUniqueId() const
karto::ScanSolver::IdPoseVector corrections
virtual void AddConstraint(karto::Edge< karto::LocalizedRangeScan > *pEdge)
virtual const karto::ScanSolver::IdPoseVector & GetCorrections() const
Vertex< T > * GetSource() const
virtual void AddNode(karto::Vertex< karto::LocalizedRangeScan > *pVertex)
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
int addNode(const Vector3d &pos, int id)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool addConstraint(int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec)
const Pose2 & GetPoseDifference()
kt_double GetHeading() const