void ImageViewer::connectWidgets(){
connect(startBtn, SIGNAL(clicked()), model, SLOT(doRegistration()));
connect(model, SIGNAL(processingDone()), this, SLOT(enableQuery()));
connect(queryBtn, SIGNAL(clicked()), model, SLOT(doQuery()));
connect(model, SIGNAL(queryDone()), this, SLOT(showCorrespondingPoints()));
connect(matchingBtn, SIGNAL(clicked()), model, SLOT(doMatching()));
connect(model, SIGNAL(matchingDone()), this, SLOT(showMatchingResult()));
}
bool Connection::createConnection()
{
bool rval = false;
struct sockaddr_in remote = {};
remote.sin_port = htons(m_port);
remote.sin_family = AF_INET;
if (inet_aton(m_address.c_str(), (struct in_addr*)&remote.sin_addr.s_addr) == 0)
{
m_error = "Invalid address: ";
m_error += m_address;
}
else
{
int fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
if (fd == -1)
{
char err[ERRBUF_SIZE];
m_error = "Failed to create socket: ";
m_error += strerror_r(errno, err, sizeof (err));
}
m_fd = fd;
if (connect(fd, (struct sockaddr*) &remote, sizeof (remote)) == -1)
{
char err[ERRBUF_SIZE];
m_error = "Failed to connect: ";
m_error += strerror_r(errno, err, sizeof (err));
}
else if (doAuth())
{
rval = doRegistration();
}
}
return rval;
}
void ThreadLocalize::eventLoop(void)
{
_sleepCond.wait(_sleepMutex);
while(_stayActive)
{
if(!_laserData.size())
{
_sleepCond.wait(_sleepMutex);
}
_dataMutex.lock();
vector<float> ranges = _laserData.front()->ranges;
if(_reverseScan)
std::reverse(ranges.begin(),ranges.end());
_stampLaserOld = _stampLaser;
_stampLaser = _laserData.front()->header.stamp;
_sensor->setRealMeasurementData(ranges);
_sensor->setStandardMask();
for(std::deque<sensor_msgs::LaserScan*>::iterator iter = _laserData.begin(); iter < _laserData.end(); iter++)
delete *iter;
_laserData.clear();
_dataMutex.unlock();
//odom rescue
// if(_useOdomRescue)
// _odomAnalyzer->odomRescueUpdate();
//std::cout << __PRETTY_FUNCTION__ << "_odomTfIsValid = " << _odomTfIsValid << std::endl;
const unsigned int measurementSize = _sensor->getRealMeasurementSize();
if(!_scene) //first call, initialize buffers
{
_scene = new double[measurementSize * 2];
_maskS = new bool[measurementSize];
_modelCoords = new double[measurementSize * 2];
_modelNormals = new double[measurementSize * 2];
_maskM = new bool[measurementSize];
*_lastPose = _sensor->getTransformation();
}
//reconstruction
unsigned int validModelPoints = _rayCaster->calcCoordsFromCurrentViewMask(&_grid, _sensor, _modelCoords, _modelNormals, _maskM);
if(validModelPoints == 0)
{
ROS_ERROR_STREAM("Localizer (" << _nameSpace << ") error! Raycasting found no coordinates! \n");
continue;
}
//get current scan
const unsigned int validScenePoints = _sensor->dataToCartesianVectorMask(_scene, _maskS);
/**
* Create Point Matrices with structure [x1 y1; x2 y2; ..]
* M, N and S are matrices that preserve the ray model of a laser scanner
* Xvalid matrices are matrices that do not preserve the ray model but contain only valid points (mask applied)
* T transformation
*/
obvious::Matrix M(measurementSize, 2, _modelCoords);
obvious::Matrix N(measurementSize, 2, _modelNormals);
obvious::Matrix S(measurementSize, 2, _scene);
obvious::Matrix Mvalid = maskMatrix(&M, _maskM, measurementSize, validModelPoints);
obvious::Matrix Nvalid = maskMatrix(&N, _maskM, measurementSize, validModelPoints);
obvious::Matrix Svalid = maskMatrix(&S, _maskS, measurementSize, validScenePoints);
obvious::Matrix T(3,3);
T = doRegistration(_sensor, &M, &Mvalid, &N, &Nvalid, &S, &Svalid);
//analyze registration result
_tf.stamp_ = ros::Time::now();
const bool regErrorT = isRegistrationError(&T, _trnsMax, _rotMax);
if(regErrorT)
{
ROS_ERROR_STREAM("Localizer(" << _nameSpace << ") registration error! \n");
sendNanTransform();
}
else //transformation valid -> transform sensor and publish new sensor pose
{
// obvious::Matrix Tbla(3, 3);
// Tbla = this->tfToObviouslyMatrix3x3(_tfFrameSensorMount);
// std::cout << __PRETTY_FUNCTION__ << "tbla" << std::endl;
// Tbla.print();
// std::cout << std::endl;
//Tbla.invert();
//T = Tbla * T;
_sensor->transform(&T);
//_sensor->transform(&Tbla);
obvious::Matrix curPose = _sensor->getTransformation();
sendTransform(&curPose);
//update map if necessary
if(this->isPoseChangeSignificant(_lastPose, &curPose))
{
*_lastPose = curPose;
_mapper.queuePush(_sensor);
}
}
}
}
