bool localizerGVG::processMeetpoint(localizer::UpdateFilter::Request &req,
localizer::UpdateFilter::Response &res) {
ROS_INFO("processMeetpoint received meetpoint %d [%lf,%lf,%lf]", req.id, req.x, req.y,req.yaw);
ROS_INFO("processMeetpoint robot pose [%lf,%lf,%lf]",X(0),X(1),X(2));
if(nL==0) {
//Reset the odometry covariance.
P=MatrixXd::Zero(3,3);
}
// Propagate the cross-corelation between robot and the
// existing landmarks up to now
propagateRL();
// For the new landmark
Vector3d xl; //the coordinates of the landmark (meetpoint) in World Frame
Vector3d z; // Vector representation of the measurment
z<<req.x, req.y, req.yaw;
xl.segment(0,2)=X.segment(0,2)+C(X(2))*z.segment(0,2);
xl(2)=X(2)+z(2);
// Create the measurment matrices
MatrixXd Hr(3,3);
Vector2d dx;
dx=xl.segment(0,2)-X.segment(0,2);
double _c=cos(X(2));
double _s=sin(X(2));
Hr<<-_c, -_s, + _c*dx(1)-_s*dx(0),
_s, -_c, - _s*dx(1)-_c*dx(0),
0, 0, -1;
Matrix3d Hl; // Hl is a 2 by 2 Matrix
Hl=MatrixXd::Zero(3,3);
Hl.block(0,0,2,2)=CT(X(2));
Hl(2,2)=1;
if(newLandmark(req.id)) {
// If pre-fix on loaded map, need to mark that this new node is in current frame
if (!mapLoadLocalization) preFixIdList.push_back(req.id);
// New Landmark
idList.push_back(req.id);
P.conservativeResize(P.rows()+3,P.cols()+3);
X.conservativeResize(X.size()+3);
X.segment(3+nL*3,3)=xl;
// Create new Pllcv matrices describing the cross corelation
// between the old landmarks and the new Landmark. Page 79 eq. 5.73
for (int count = 0; count < nL; count++) {
Matrix3d newPllcv;
newPllcv = -P.block(3 + count*3, 0, 3, 3)*Hr.transpose()*Hl;
P.block(3 + count*3, 3 + nL*3, 3, 3) = newPllcv;
P.block(3 + nL*3, 3 + count*3, 3, 3) = newPllcv.transpose();
}
// Create new Prl matrix describing the cross corelation
// between the Robot and the new Landmark. Page 79 eq. 5.73
MatrixXd newPrl(3,3);
newPrl=-P.block(0,0,3,3)*Hr.transpose()*Hl;
P.block(0,3+nL*3,3,3)=newPrl;
P.block(3+nL*3,0,3,3)=newPrl.transpose();
// Create the new Pll matrix describing the covariance of the landmark
// page 79 eq. 5.74
Matrix3d newPll;
newPll=Hl.transpose()*(Hr*P.block(0,0,3,3)*Hr.transpose()+R)*Hl;
P.block(3+nL*3,3+nL*3,3,3)=newPll;
nL++;
}
else {
// Find which landmark it is:
int id=-1;
for(unsigned int i=0; i<idList.size(); i++) {
if(idList[i] == req.id) id=i;
}
//Sanity check:
if(id < 0) {
cerr<<"Not able to find id:"<<req.id<< "in the list of ids"<<endl;
for(unsigned int i=0; i<idList.size(); i++){
cerr<<idList[i]<<" ";
}
cerr<<endl;
}
// Check if the revisited node was one from previous map or current map
// If from current map, don't use this node to compute transformation between old/new map
bool oldMapNode = true;
for (vector<int>::iterator it = preFixIdList.begin(); it != preFixIdList.end(); ++it){
if((*it)==id) {
oldMapNode = false;
break;
}
}
if (!mapLoadLocalization && oldMapNode) {
// Copy the covariance information to Prr and also Xr
Vector3d offset;
offset(2) = X(2)-bearing_angle;
Vector2d v;
v(0) = X(3*id+3);
v(1) = X(3*id+4);
Vector2d rotated_mp;
rotated_mp = C(offset(2))*v;
offset(0) = X(0)-rotated_mp(0);
offset(1) = X(1)-rotated_mp(1);
/*P.block(0,0,3,3) = P_init.block(3*id+3, 3*id+3, 3, 3);
// Copying information to Prl
P.block(0,3,3,3*nL) = P.block(3*id+3, 3, 3, 3*nL);
P.block(3,0,3*nL,3) = P.block(3, 3*id+3, 3*nL, 3);*/
mapLoadLocalization = true;
// Calculate translation between current frame and loaded map frame
for (unsigned int i = 0; i < nL; i++) {
bool corrected = false;
for (vector<int>::iterator it = preFixIdList.begin(); it != preFixIdList.end(); ++it){
if((*it)==i) {
corrected = true;
break;
}
}
if (!corrected) {
Vector2d v;
v(0) = X(3*i+3);
v(1) = X(3*i+4);
Vector2d result;
result = C(offset(2))*v;
X(3*i+3) = result(0);
X(3*i+4) = result(1);
X(3*i+3) = X(3*i+3) + offset(0);
X(3*i+4) = X(3*i+4) + offset(1);
X(3*i+5) = thetapp(X(3*i+5) + offset(2));
}
}
P = P_init;
X = X_init;
}
else {
// perform update
ROS_INFO("localizerGVG::processMeetpoint id %d %d",req.id, id);
Vector3d z_est;
z_est.segment(0,2)=CT(X(2))*(X.segment(3+3*id,2)-X.segment(0,2));
z_est(2)=X(3+3*id+2)-X(2);
Vector3d r;
r.segment(0,2)=z.segment(0,2)-z_est.segment(0,2);
r(2)=angleDiff(z(2),z_est(2));
MatrixXd H(3,3+3*nL);
H=MatrixXd::Zero(3,3+3*nL);
H.block(0,0,3,3)=Hr;
H.block(0,3+3*id,3,3)=Hl;
MatrixXd S(3,3);
S=H*P*H.transpose()+R;
MatrixXd K(3,3+3*nL);
K=P*H.transpose()*S.inverse();
X=X+K*r;
X(2)=thetapp(X(2));
P=(MatrixXd::Identity(3+nL*3,3+nL*3)-K*H)*P*(MatrixXd::Identity(3+nL*3,3+nL*3)-K*H).transpose()+K*R*K.transpose();
}
}
// publish the odometry with covariance topic
// Publish the Map
localizer::GVGmap theMap;
for(unsigned int i=0; i<X.size(); i++) {
theMap.state.push_back(X[i]);
res.X.push_back(X[i]);
}
for(unsigned int i=0; i<P.rows(); i++) {
for(unsigned int j=0; j<P.cols(); j++) {
theMap.cov.push_back(P(i,j));
res.P.push_back(P(i,j));
}
}
mapPub.publish(theMap);
return(true);
}
