Go to the documentation of this file.
65 v->pose.y()-
v->parent->pose.y(),
66 v->pose.theta()-
v->parent->pose.theta());
70 TreeOptimizer2::TreeOptimizer2():
125 R.values[0][0]=
t.rotationMatrix[0][0];
126 R.values[0][1]=
t.rotationMatrix[0][1];
129 R.values[1][0]=
t.rotationMatrix[1][0];
130 R.values[1][1]=
t.rotationMatrix[1][1];
140 for (
int dir=0; dir<2; dir++){
141 Vertex*
n = (dir==0)?
e->v1 :
e->v2;
144 M[
i].values[0]+=
W.values[0][0];
145 M[
i].values[1]+=
W.values[1][1];
146 M[
i].values[2]+=
W.values[2][2];
157 cerr <<
"M[" <<
i <<
"]=" <<
M[
i].x() <<
" " <<
M[
i].y() <<
" " <<
M[
i].theta() <<endl;
225 double tw[3]={0.,0.,0.};
226 for (
int dir=0; dir<2; dir++) {
227 Vertex*
n = (dir==0)?
v1 :
v2;
230 tw[0]+=1./
M[
i].values[0];
231 tw[1]+=1./
M[
i].values[1];
232 tw[2]+=1./
M[
i].values[2];
245 for (
int dir=0; dir<2; dir++) {
246 Vertex*
n = (dir==0)?
v1 :
v2;
247 double sign=(dir==0)? -1. : 1.;
259 n->parameters.x()+=
delta.x();
260 n->parameters.y()+=
delta.y();
261 n->parameters.theta()+=
delta.theta();
293 v->pose.x()=
v->parent->pose.x()+
v->parameters.x();
294 v->pose.y()=
v->parent->pose.y()+
v->parameters.y();
295 v->pose.theta()=
v->parent->pose.theta()+
v->parameters.theta();
304 p.x()+=aux->parameters.x();
305 p.y()+=aux->parameters.y();
306 p.theta()+=aux->parameters.theta();
309 p.x()+=aux->pose.x();
310 p.y()+=aux->pose.y();
311 p.theta()+=aux->pose.theta();
317 const Vertex*
v1=
e->v1;
318 const Vertex*
v2=
e->v2;
358 return r.
x()*r1.
x()+r.y()*r1.
y()+r.theta()*r1.
theta();
362 double globalError=0.;
363 for (TreePoseGraph2::EdgeMap::const_iterator it=
edges.begin(); it!=
edges.end(); it++){
364 globalError+=
error(it->second);
virtual ~TreeOptimizer2()
Operations2D< double >::PoseType Pose
void updatePoseChain(Vertex *v, Vertex *top)
void initializeTreeParameters()
const EIGEN_DEVICE_FUNC SignReturnType sign() const
std::multiset< Edge *, EVComparator< Edge * > > EdgeSet
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index rows, Index cols)
double beta(double a, double b)
void iterate(TreePoseGraph2::EdgeSet *eset=0)
void initializeOnlineOptimization()
void initializeOptimization()
T values[3]
container for x, y, and theta
A class to represent symmetric 3x3 matrices.
for(int i=0;i< std::min(*m, *n);++i) ipiv[i]++
Pose getPose(Vertex *v, Vertex *top)
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
2D Point (x,y) with orientation (theta)
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
void perform(TreePoseGraph2::Vertex *v)
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
void treeDepthVisit(Action &act, Vertex *v)
A class (struct) to compute the parameterization of the vertex v.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GLM_FUNC_DECL genType cos(genType const &angle)
void computePreconditioner()
Array< int, Dynamic, 1 > v
GLM_FUNC_DECL genType sin(genType const &angle)
rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:22