BOOST_FOREACH(boost::shared_ptr<count_writer_t> p_count_writer, mCellPopulationCountWriters)
{
p_count_writer->OpenOutputFile(rOutputFileHandler);
p_count_writer->WriteHeader(this);
}
inline std::vector<int> const &ProblemData::getSamplesClasses() const
{
return *ptrSamplesClasses.get();
}
void ribi::gtst::ParticipantDialogStateChooseAction::RespondToTimedServerPush()
{
const int time_left = m_server->GetStates()->GetCurrentState()->GetTimeLeft();
//Update label_time_left
{
std::string text
= std::string("Time left: ")
+ std::to_string(time_left)
+ std::string(" seconds");
if (m_server->GetParameters()->GetChooseAction()->GetWait())
{
//Infinite time
text+=std::string(" (waiting for others)");
}
assert(ui.m_label_time_left);
ui.m_label_time_left->setText(text.c_str());
}
//Check if choice must be sent to the server
if (!m_server->GetParameters()->GetChooseAction()->GetWait()
&& time_left <= 0)
{
assert(GetDialog());
assert(GetDialog()->CanGetParticipant());
if (ui.m_button_choose_action->isEnabled())
{
//Let Participant choose a random action by time
const int selected = ui.m_group->selectedButtonIndex();
assert(ui.m_group->count() > 0);
const int action_index
= (selected == -1 //Did user select something?
? std::rand() % ui.m_group->count() //Take a random action
: selected);
assert(action_index > -1 && action_index < ui.m_group->count());
ui.m_group->setSelectedButtonIndex(action_index);
OnChooseActionClick();
}
}
if (!ui.m_button_choose_action->isEnabled())
{
assert(ui.m_group->selectedButtonIndex() != -1
&& "m_button_choose_action should only disable when an action is selected");
const std::vector<boost::shared_ptr<ChooseActionOption> >& options
= m_server->GetParameters()->GetChooseAction()->GetOptions();
assert(ui.m_group->selectedButtonIndex() < static_cast<int>(options.size()));
const boost::shared_ptr<ChooseActionOption> option
= options[ ui.m_group->selectedButtonIndex() ];
const std::string text
= option->GetMessageChoice()
+ std::string("\', waiting for the others...");
ui.m_label_status->setText(text.c_str());
}
else
{
ui.m_label_status->setText("Please choose an action.");
}
///Follow the server its tempo
RespondToParticipant();
}
template<typename F, typename G> locked_ptr(boost::shared_ptr<T> pointer, F lock, G unlock):
boost::shared_ptr<T>(pointer.get(), unlock),
value(pointer)
{
lock();
};
inline boost::unordered_map<std::string, int> const &ProblemData::getClassMap() const
{
return *ptrClassMap.get();
}
void ribi::TriangleMeshCreatorMenuDialog::TestDeep() noexcept
{
const bool verbose{false};
if (verbose) { TRACE("Trying out to build cells from the hardest testing templates"); }
{
using ribi::trim::Cell;
using ribi::trim::CellsCreator;
using ribi::trim::CellsCreatorFactory;
using ribi::trim::CreateVerticalFacesStrategy;
using ribi::trim::CreateVerticalFacesStrategies;
using ribi::trim::Dialog;
using ribi::trim::Template;
//This is the longest test by far
//const TestTimer test_timer(boost::lexical_cast<std::string>(__LINE__),__FILE__,1.0);
for (CreateVerticalFacesStrategy strategy: CreateVerticalFacesStrategies().GetAll())
{
const boost::shared_ptr<Template> my_template {
Template::CreateTest(3)
};
const int n_cell_layers = 2;
const boost::shared_ptr<CellsCreator> cells_creator{
CellsCreatorFactory().Create(
my_template,
n_cell_layers,
1.0 * boost::units::si::meter,
strategy,
verbose
)
};
const std::vector<boost::shared_ptr<Cell>> cells { cells_creator->GetCells() };
assert(cells.size() > 0);
}
}
if (verbose) { TRACE("Testing case 1"); }
TriangleMeshCreatorMenuDialog().Execute(
{
"TriangleMeshCreator",
"--layer_height", "1",
"--WKT", "POLYGON((1 1,-1 1,-1 -1,1 -1))",
"--strategy", "1",
"--n_layers", "1",
"--fraction", "0.75",
//"--show_mesh",
//"--verbose",
"--triangle_max_area", "10.0",
"--triangle_min_angle", "20.0",
"--profile"
}
);
if (verbose) { TRACE("Testing case 2"); }
TriangleMeshCreatorMenuDialog().Execute(
{
"TriangleMeshCreator",
"-z", "1",
"-w", "POLYGON((0 0,0 3,3 0)),POLYGON((1 1,0 2,2 0))",
"-s", "1",
"-n", "1",
"-f", "0.75",
//"-m",
//"-b",
"--triangle_area", "10.0",
"--triangle_quality", "20.0",
"--profile"
}
);
if (verbose) { TRACE("Testing case 3"); }
TriangleMeshCreatorMenuDialog().Execute(
{
"TriangleMeshCreator",
"-z", "1",
"-w", "POLYGON((10 10,10 -10,-10 -10,-10 10)),LINESTRING(5 5,5 -5,-5 -5,-5 5)",
"-s", "1",
"-n", "1",
"-f", "0.75",
//"-m",
//"-b",
"-r", "10.0",
"-q", "20.0",
"--profile"
}
);
if (verbose) { TRACE("Testing case 4"); }
TriangleMeshCreatorMenuDialog().Execute(
{
"TriangleMeshCreator",
"-z", "1",
"-w", "LINESTRING(5 5,5 -5,-5 -5,-5 5)",
"-s", "1",
"-n", "1",
"-f", "0.75",
//"-m",
//"-b",
"-r", "10.0",
"-q", "20.0",
"--profile"
}
);
if (verbose) { TRACE("Testing mesh of issue #221"); }
TriangleMeshCreatorMenuDialog().Execute(
{
"TriangleMeshCreator",
"--layer_height","1",
"--WKT", "POLYGON((10 10,10 -10,-10 -10,-10 10))",
"--strategy", "1",
"--n_layers", "10",
"--fraction", "0.9",
"--triangle_max_area", "0.1",
"--triangle_min_angle", "20.0",
"--show_mesh",
"--profile"
}
);
if (verbose) { TRACE("Tested all cases"); }
}
///////////////////////////////////////////////////////////////////
///Syncronize objects only for clouds
//////////////////////////////////////////////////////////////////
void Callback(const sensor_msgs::PointCloud2ConstPtr& cloudI)
{
ROS_INFO("cloud timestamps %i.%i ", cloudI->header.stamp.sec,cloudI->header.stamp.nsec);
//////////////////////////////////////////////////////////////////
///for range images
/////////////////////////////////////////////////////////////////
Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::LASER_FRAME;
float noiseLevel=0.00;
float minRange = 0.0f;
int borderSize = 1;
/////////////////////////////////////////////////////////////////////
/// point cloud
///////////////////////////////////////////////////////////////////
PointCloud<PointXYZ>::Ptr cloud_ptr (new PointCloud<PointXYZ>);
if ((cloudI->width * cloudI->height) == 0)
return ;
ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
(int)cloudI->width * cloudI->height,
cloudI->header.frame_id.c_str (),
pcl::getFieldsList (*cloudI).c_str ());
pcl::fromROSMsg(*cloudI, *cloud_ptr);
///segment data only the part in front of
SegmentData(cloud_ptr,cloud_filtered,cloud_nonPlanes,cloud_Plane,cloud_projected);
///////////////////////////////////////////////
///image range images
//////////////////////////////////////////////
range_image->createFromPointCloud (*cloud_filtered, pcl::deg2rad(0.2f),
pcl::deg2rad (360.f), pcl::deg2rad (180.0f),
sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
/////////////////////////////////////////////////////////////////////////////////////////////
///planar range image TESTING
//////////////////////////////////////////////////////////////////////////////////////////
Eigen::Affine3f trans,P_affine;//does not include K
trans (0,0)= 0.0f; trans (0,1)= 0.0f; trans (0,2)=-1.0f; trans(0,3)=0.0f;
trans (1,0)= 0.0f; trans (1,1)= 1.0f; trans (1,2)=0.0f; trans (1,3)=0.0f;
trans (2,0)= 1.0f; trans (2,1)= 0.0f; trans (2,2)=0.0f; trans (2,3)=0.0f;
trans (3,0)= 0.0f; trans (3,1)= 0.0f; trans (3,2)=0.0f; trans (3,3)=1.0f;
P_affine (0,0)= 0.0187f; P_affine (0,1)= -0.8024f; P_affine (0,2)= -0.5965f; P_affine(0,3)=-0.7813f;
P_affine (1,0)= -0.9998f; P_affine (1,1)= -0.0168f; P_affine (1,2)=-0.0088f; P_affine (1,3)=0.1818f;
P_affine (2,0)= -0.0030f; P_affine (2,1)= 0.5965f; P_affine (2,2)=-0.8026f; P_affine (2,3)=0.7670f;
P_affine (3,0)= 0.0f; P_affine (3,1)= 0.0f; P_affine (3,2)=0.0f; P_affine (3,3)=1.0f;
// 0.0187 -0.8024 -0.5965 -0.7813
// -0.9998 -0.0168 -0.0088 0.1818
// -0.0030 0.5965 -0.8026 0.7670
for(unsigned int i=0; i<cloud_filtered->size();i++)
{
PointXYZ p1,p2;
p1=cloud_filtered->points[i];
p2.x=-p1.z;
p2.y=p1.y;
p2.z=p1.x;
cloud_trans->push_back(p2);
}
// pcl::transformPointCloud (*cloud_filtered, *cloud_filtered, trans);
Eigen::Affine3f pose(Eigen::Affine3f::Identity());
// pose=pose*Translation3f(T_);
pose=P_affine;
///
range_image_planar->createFromPointCloudWithFixedSize(*cloud_filtered,768, 1024,
//K_(0,2),K_(1,2),K_(0,0),K_(1,1),P_affine
K_(0,2),K_(1,2),K_(0,0),K_(1,1), Eigen::Affine3f::Identity()
,pcl::RangeImage::LASER_FRAME, noiseLevel, minRange);
//GenerarImagenOpenCV(range_image,im_range_current, im_range_current_norm);
//TESTING PART with planar range image
GenerarImagenOpenCV(range_image_planar,im_range_current, im_range_current_norm);
applyColorMap( im_range_current_norm, falseColorsMap, COLORMAP_HOT);
if(!firstFrame)
{
Mat im2 = Mat(im_range_previous_norm.rows, im_range_previous_norm.cols, im_range_previous_norm.type());
cv::resize(im_range_current_norm, im2,im2.size(), INTER_LINEAR );
cv::calcOpticalFlowFarneback(im2,im_range_previous_norm,flow,0.5, 3, 15, 3, 5, 1.2, 0);
///show data
cv::cvtColor( im2 ,cflow, CV_GRAY2BGR);
drawOptFlowMap(flow, cflow, 10, 1.5, Scalar(0, 255, 0));
drawOpticalFlow_color(flow,color_flow, -1);
//Set_range_flow(range_image,im2,flow,cloud_flow,cloud_p3d,normals,cloud_rgb);
///planar range image TESTING
Set_range_flow(range_image_planar,im2,flow,cloud_flow,cloud_p3d,normals,cloud_rgb);
////////////////////////////////////////////////////////////////////////////////////
///publishing
//////////////////////////////////////////////////////////////////////////////////
cv_bridge::CvImagePtr cv_send(new cv_bridge::CvImage);
cv_send->image=Mat(falseColorsMap.rows, falseColorsMap.cols, CV_8UC3);
cv_send->encoding=enc::RGB8;
///range image
falseColorsMap.copyTo(cv_send->image);
pub_im_range.publish(cv_send->toImageMsg());
//flow arrows
cv_send->image=Mat(cflow.rows, cflow.cols, CV_8UC3);
cflow.copyTo(cv_send->image);
pub_im_range_flow.publish(cv_send->toImageMsg());
///color flow
cv_send->image=Mat( color_flow.rows, color_flow.cols, CV_8UC3);
color_flow.copyTo(cv_send->image);
pub_im_range_color_flow.publish(cv_send->toImageMsg());
///cloud flow
sensor_msgs::PointCloud2 output_cloud;
pcl::toROSMsg(*cloud_rgb,output_cloud);
output_cloud.header.frame_id = cloudI->header.frame_id;
pub_cloud_flow.publish(output_cloud);
////////////////////////////////////
///cloud complete
///////////////////////////////////////
pub_cloud.publish(cloudI);
////////////////////////////////////////////
///point normal
/////////////////////////////////////////////
sensor_msgs::PointCloud2 output_cloud2;
pcl::concatenateFields(*cloud_p3d,* normals,*cloud_normals);
pcl::toROSMsg(*cloud_normals,output_cloud2);
output_cloud2.header.frame_id = cloudI->header.frame_id;
pub_cloud_normal.publish(output_cloud2);
//////////////////////////////////////////////////////////////
///markers
///////////////////////////////////////////////////////////
visualization_msgs::Marker points,line_list;
Publish_Marks(cloud_flow,cloud_p3d, points,line_list);
pub_markers.publish(points);
pub_markers.publish(line_list);
}
//create the images
firstFrame=0;// we have read the first frame
im_range_current_norm.copyTo(im_range_previous_norm);
flow=Mat(im_range_current_norm.rows, im_range_current_norm.cols,CV_32FC2);
cflow=Mat(im_range_current_norm.rows, im_range_current_norm.cols, CV_8UC3);
}
void VectorPort::onChildCreate(
const boost::shared_ptr<BaseVectorType::ChildType>& inCreatedChild) const throw (Error) {
inCreatedChild->setName(getName());
}
Connection::Connection(boost::shared_ptr< Hive > hive)
: m_hive(hive), m_socket(hive->GetService()), m_io_strand(hive->GetService()), m_timer(hive->GetService()), m_receive_buffer_size(4096), m_timer_interval(1000), m_error_state(0)
{
}
OverlayPluginTestFixture()
{
arrayPool = new NDArrayPool(100, 0);
expectedArrayCounter=0;
// Asyn manager doesn't like it if we try to reuse the same port name for multiple drivers
// (even if only one is ever instantiated at once), so we change it slightly for each test case.
std::string simport("simOVER1"), testport("OVER1");
uniqueAsynPortName(simport);
uniqueAsynPortName(testport);
// We need some upstream driver for our test plugin so that calls to connectArrayPort
// don't fail, but we can then ignore it and send arrays by calling processCallbacks directly.
driver = boost::shared_ptr<asynPortDriver>(new asynPortDriver(simport.c_str(),
1, 1,
asynGenericPointerMask,
asynGenericPointerMask,
0, 0, 0, 2000000));
// This is the plugin under test
Overlay = boost::shared_ptr<OverlayPluginWrapper>(new OverlayPluginWrapper(testport.c_str(),
50,
1,
simport.c_str(),
0,
8,
0,
0,
0,
1));
// This is the mock downstream plugin
downstream_plugin = new TestingPlugin(testport.c_str(), 0);
// Enable the plugin
Overlay->start(); // start the plugin thread although not required for this unittesting
Overlay->write(NDPluginDriverEnableCallbacksString, 1);
Overlay->write(NDPluginDriverBlockingCallbacksString, 1);
client = boost::shared_ptr<asynGenericPointerClient>(new asynGenericPointerClient(testport.c_str(), 0, NDArrayDataString));
client->registerInterruptUser(&Overlay_callback);
// Test a "normal" case
overlayTempCaseStr test1 = {0, 500, 500, 50, 50, 1, 1, 0, 255, 0, NDOverlayCross, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
appendTestCase(&overlayTestCaseStrs, &test1);
// Test a case with size larger than array
overlayTempCaseStr test2 = {0, 500, 500, 5000, 5000, 1, 1, 0, 255, 0, NDOverlayEllipse, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
appendTestCase(&overlayTestCaseStrs, &test2);
// Test a case with zero size
overlayTempCaseStr test3 = {0, 500, 500, 0, 0, 1, 1, 0, 255, 0, NDOverlayRectangle, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
appendTestCase(&overlayTestCaseStrs, &test3);
// Test a case with negative position
overlayTempCaseStr test4 = {0, -500, -500, 50, 50, 1, 1, 0, 255, 0, NDOverlayCross, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
appendTestCase(&overlayTestCaseStrs, &test4);
// Test a case with position larger than array size
overlayTempCaseStr test5 = {0, 1500, 1500, 50, 50, 1, 1, 0, 255, 0, NDOverlayEllipse, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
appendTestCase(&overlayTestCaseStrs, &test5);
// Test an RGB1 case
overlayTempCaseStr test6 = {0, 500, 500, 50, 50, 1, 1, 0, 255, 0, NDOverlayRectangle, NDOverlaySet, 3, {3, 1024, 1024}, NDColorModeRGB1};
appendTestCase(&overlayTestCaseStrs, &test6);
// Test an case with very large width
overlayTempCaseStr test7 = {0, 500, 500, 50, 50, 5000, 5000, 0, 255, 0, NDOverlayCross, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
appendTestCase(&overlayTestCaseStrs, &test7);
// Test an case with zero width
overlayTempCaseStr test8 = {0, 500, 500, 50, 50, 0, 0, 0, 255, 0, NDOverlayCross, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
appendTestCase(&overlayTestCaseStrs, &test8);
// Test a "normal" case using address 1
overlayTempCaseStr test9 = {1, 500, 500, 50, 50, 1, 1, 0, 255, 0, NDOverlayCross, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
appendTestCase(&overlayTestCaseStrs, &test9);
}
void Add( const boost::shared_ptr<T>& prt )
{
if ( prt_.empty() )
prt->SetFocus();
prt_.push_back(prt);
}
void print_fun(boost::shared_ptr<int> value)
{cout<<"fun : value is = "<<*value<<", use_count = "<<value.use_count()<<endl;}
void print()
{cout<<"class : value is = "<<*p_<<", use_count = "<<p_.use_count()<<endl;}
void OgrWriter::writePartial(const boost::shared_ptr<const hoot::Relation>& newRelation)
{
// Make sure all the elements in the relation are in the cache
const std::vector<RelationData::Entry>& relationEntries = newRelation->getMembers();
std::vector<RelationData::Entry>::const_iterator relationElementIter;
long nodeCount = 0;
long wayCount = 0;
long relationCount = 0;
for ( relationElementIter = relationEntries.begin();
relationElementIter != relationEntries.end();
relationElementIter++ )
{
switch ( relationElementIter->getElementId().getType().getEnum() )
{
case ElementType::Node:
nodeCount++;
if ( nodeCount > _currElementCacheCapacity )
{
LOG_FATAL("Relation ID " << newRelation->getId() <<
" contains more nodes than can fit in the cache (" << _currElementCacheCapacity<<
")");
throw HootException("Relation with too many nodes");
}
break;
case ElementType::Way:
wayCount++;
if ( wayCount > _currElementCacheCapacity)
{
LOG_FATAL("Relation ID " << newRelation->getId() <<
" contains more ways than can fit in the cache (" << _currElementCacheCapacity <<
")");
throw HootException("Relation with too many ways");
}
break;
case ElementType::Relation:
relationCount++;
if ( relationCount > _currElementCacheCapacity)
{
LOG_FATAL("Relation ID " << newRelation->getId() <<
" contains more relations than can fit in the cache (" << _currElementCacheCapacity <<
")");
throw HootException("Relation with too many relations");
}
break;
default:
throw HootException("Relation containus unknown type");
break;
}
if ( _elementCache->containsElement(relationElementIter->getElementId()) == false )
{
throw HootException("Relation element did not exist in cache");
}
}
// Add to the cache
ConstElementPtr constRelation(newRelation);
_elementCache->addElement(constRelation);
ElementProviderPtr cacheProvider(_elementCache);
_writePartial(cacheProvider, newRelation);
}
cql::cql_message_query_impl_t::cql_message_query_impl_t(const boost::shared_ptr<cql_query_t>& query)
: _buffer(new std::vector<cql_byte_t>())
, _consistency(query->consistency())
, _query(query->query())
, _is_traced(query->is_traced())
{}
Acceptor::Acceptor(boost::shared_ptr< Hive > hive)
: m_hive(hive), m_acceptor(hive->GetService()), m_io_strand(hive->GetService()), m_timer(hive->GetService()), m_timer_interval(1000), m_error_state(0)
{
}
void PeekProcessor::peek(boost::shared_ptr<TProtocol> in,
TType ftype,
int16_t fid) {
(void) fid;
in->skip(ftype);
}
void Acceptor::DispatchAccept(boost::shared_ptr< Connection > connection)
{
m_acceptor.async_accept(connection->GetSocket(), connection->GetStrand().wrap(boost::bind(&Acceptor::HandleAccept, shared_from_this(), _1, connection)));
}
ConsistencyTests()
: ccm(new CCM::Bridge("config.txt"))
, ip_prefix(ccm->get_ip_prefix()) {}
void compare_scans(boost::shared_ptr<vector<line> > firstScan,
boost::shared_ptr<vector<line> > secondScan,
int rotation_prior_mean,
int& rotOut,
Vector2f translation_prior_mean,
Vector2f & translationOut)
{
int num_theta_entries = (2 * M_PI -
delta_theta*angle_increment)/(delta_theta*M_PI/180) + 1;
vector<float> theta = vector<float>(num_theta_entries);
for(double i = -M_PI, counter = 0; counter < num_theta_entries;
i+= delta_theta*M_PI/180, counter++) {
theta[counter] = i;
}
//Initial rotation estimation. This will vary depending on the
//lines being compared.
// Vector2f translation_prior_mean = previous_translation;
float translation_prior_sd = 0.5;
// float rotation_prior_mean = previous_est_rot;
float rotation_prior_sd = 15;
float rotation_posterior_sd = 4;
vector<float> rotation_prior (360,0);
vector<float> rotation_score (360,0);
float dist;
float sum1;
for (int i=0; i<360; i++)
{
dist = min(abs(i - rotation_prior_mean),abs(i-360-rotation_prior_mean));
rotation_prior[i] = exp(-pow(dist,2)/pow(rotation_prior_sd,2));
sum1 += rotation_prior[i];
}
for (int i=0; i<360; i++)
{
rotation_prior[i] /= sum1;
}
//for each pair of lines between the two scans, estimate the
// probability that they are from the same object. The more likely they
// are to be by an unmoved object, the more likely that the rotation of
// the was equal to the theta difference between the lines.
for (int i=0; i < firstScan->size(); i++)
{
for (int j=0; j < secondScan->size(); j++)
{
float dist;
float theta_diff = (delta_theta*(firstScan->at(i).theta_index -
secondScan->at(j).theta_index) );
if (theta_diff < 0) {
theta_diff += 360;
}
//if the lines are clearly not from the same source because the
// translation has moved too much for one frame, then do not use their
// relative angles to estimate the rotation.
float expected_rho_change;
expected_rho_change =
-(translation_prior_mean[0]*cos(theta[firstScan->at(i).theta_index]) +
translation_prior_mean(1)*sin(theta[firstScan->at(i).theta_index]) );
if(abs(firstScan->at(i).est_rho + expected_rho_change -
secondScan->at(j).est_rho) > rho_sanity_tolerance)
{
continue;
}
for (int k=0; k<360; k++)
{
dist = min(abs(k-theta_diff), abs(k-theta_diff-360));
rotation_score[k] = rotation_score[k] +
exp(-pow(dist,2)/pow(rotation_posterior_sd,2));
}
}
}
vector<float> rotation_prob(360,0);
for (int i=0; i<rotation_prob.size(); i++) {
rotation_prob[i] = rotation_prior[i]* rotation_score[i];
//rotation_prob[i] = rotation_score[i];
}
float max1 = 0;
for (int i=0; i<rotation_prob.size(); i++) {
if (rotation_prob[i]> max1) {
max1 = rotation_prob[i];
rotOut = i;
}
}
cout << "est rot: " << rotOut << endl;
vector<vector<int> > matched_lines;
for (int i=0; i < firstScan->size(); i++)
{
for (int j=0; j < secondScan->size(); j++)
{
if (min(abs(delta_theta*(firstScan->at(i).theta_index -
secondScan->at(j).theta_index) - rotOut),
abs(delta_theta*(firstScan->at(i).theta_index -
secondScan->at(j).theta_index) - rotOut + 360))< 10)
{
float expected_rho_change;
expected_rho_change =
-(translation_prior_mean[0]*cos(theta[firstScan->at(i).theta_index]) +
translation_prior_mean(1)*sin(theta[firstScan->at(i).theta_index]) );
if(abs(firstScan->at(i).est_rho + expected_rho_change -
secondScan->at(j).est_rho) < rho_sanity_tolerance)
{
vector<int> temp_vector(2,0);
temp_vector[0] = i;
temp_vector[1] = j;
matched_lines.push_back(temp_vector);
break;
}
}
}
}
if (matched_lines.size() > 0)
{
//compute the change in rho for each pair of matched lines
MatrixXf rho_change(matched_lines.size(),1);
for (int i=0; i<matched_lines.size(); i++)
{
rho_change(i,0) = firstScan->at(matched_lines[i][0]).est_rho -
secondScan->at(matched_lines[i][1]).est_rho;
}
MatrixXf A(matched_lines.size(),2);
for (int i=0; i<matched_lines.size(); i++)
{
A(i,0) = cos(theta[firstScan->at(matched_lines[i][0]).theta_index]);
A(i,1) = sin(theta[firstScan->at(matched_lines[i][0]).theta_index]);
// A(i,0) = cos(theta[secondScan->at(matched_lines[i][1]).theta_index]);
// A(i,1) = sin(theta[secondScan->at(matched_lines[i][1]).theta_index]);
cout << A(i,0) << " " << A(i,1) << endl;
}
float condition_threshold = 0.2;
Matrix2f C = A.transpose()*A;
SelfAdjointEigenSolver<Matrix2f> eigensolver(C);
Vector2f Eigenvalues = eigensolver.eigenvalues();
Matrix2f Eigenvectors = eigensolver.eigenvectors();
MatrixXd observed_projection(2,2);
if (Eigenvalues(0) == 0) {
Eigenvalues(0) = 0.000001;
}
if (Eigenvalues(1) == 0) {
Eigenvalues(1) = 0.000001;
}
cout << "condition number: " << Eigenvalues(0)/Eigenvalues(1) << endl;
if (Eigenvalues(0)/Eigenvalues(1) < condition_threshold ||
Eigenvalues(0)/Eigenvalues(1) > 1/condition_threshold)
{
int principal_eigenvalue = 0;
if (Eigenvalues(1) > Eigenvalues(0))
principal_eigenvalue = 1;
MatrixXd unobserved_direction(2,1);
unobserved_direction(0) = Eigenvectors(0,1-principal_eigenvalue);
unobserved_direction(1) = Eigenvectors(1,1-principal_eigenvalue);
MatrixXd observed_direction(2,1);// = Eigenvectors(0,1,2,1);
observed_direction(0) = Eigenvectors(0,principal_eigenvalue);
observed_direction(1) = Eigenvectors(1,principal_eigenvalue);
//cout << "principal_eigenvalue: " << principal_eigenvalue << endl;
//cout << "Eigenvectors: " << endl;
//cout << Eigenvectors;
observed_projection =
observed_direction*(observed_direction.transpose()*observed_direction).inverse()*observed_direction.transpose();
}
else {
observed_projection << 1, 0,
0, 1;
}
Vector2f currentTranslation = A.jacobiSvd(ComputeThinU |
ComputeThinV).solve(rho_change);
Matrix2f observed_projection2f;
observed_projection2f(0,0) = observed_projection(0,0);
observed_projection2f(0,1) = observed_projection(0,1);
observed_projection2f(1,0) = observed_projection(1,0);
observed_projection2f(1,1) = observed_projection(1,1);
//translationOut = observed_projection2f * currentTranslation +
// (sourceTranslation - observed_projection2f*sourceTranslation);
// translationOut = observed_projection2f * currentTranslation +
// (previousTranslation - observed_projection2f*previousTranslation);
translationOut = currentTranslation;
if (rotOut > 180) {
rotOut -= 360;
}
cout << endl << "Estimated Rotation: " << rotOut;
//cout<< " Estimated Translation: " << translationOut(0) << " "
// << translationOut(1) << " MatchedLines: " << matched_lines.size() <<
//endl;
cout<< " Estimated Translation: " << base_translation(0) +
translationOut(0) << " " << base_translation(1) + translationOut(1)
<< " MatchedLines: " << matched_lines.size() << endl;
}
}
void
SessionManager::addDataprocessor( boost::shared_ptr<Dataprocessor>& proc )
{
sessions_.push_back( Session( proc ) );
emit signalSessionAdded( proc.get() );
}
~Compare() {
scan1.reset();
scan2.reset();
}
inline std::vector<std::string> const &ProblemData::getClassLabels() const
{
return *ptrClassLabels.get();
}
void lines_callback(const art_lrf::Lines::ConstPtr& msg_lines) {
if ((msg_lines->theta_index.size() != 0) &&
(msg_lines->est_rho.size() != 0) && (msg_lines->endpoints.size() !=
0)) {
scan2.reset(new vector<line> ());
line temp;
geometry_msgs::Point32 temp2;
geometry_msgs::Point32 temp_theta_index;
vector<int> temp3;
for (int i = 0; i < msg_lines->theta_index.size(); i++) {
temp.theta_index = msg_lines->theta_index[i];
temp.est_rho = msg_lines->est_rho[i];
for (int j = 0; j < msg_lines->endpoints[i].points.size(); j++) {
temp2 = msg_lines->endpoints[i].points[j];
temp3.push_back(temp2.x);
temp3.push_back(temp2.y);
temp.endpoints.push_back(temp3);
}
scan2->push_back(temp);
temp.endpoints.clear();
}
if (firstRun == true) {
firstRun = false;
scan1 = scan2;
previous_translation << 0,0;
}
else
{
compare_scans(scan1, scan2, previous_est_rot, est_rot,
previous_translation, est_translation );
if( pow(pow(est_translation[0],2) + pow(est_translation[1],2),0.5) >
WAYPOINT_THRESHOLD)
{
measurement new_base;
for(int i=0; i<scan1->size(); i++)
{
new_base.lines.push_back(scan1->at(i));
}
new_base.x = base_translation(0) + est_translation(0);
new_base.y = base_translation(1) + est_translation(1);
new_base.yaw = base_rot;
// float min_dist = 2*REMATCH_CANDIDATE_THRESH; //start with
// arbitrarily high value
// int min_index = 0;
// for( int i=0; i<base_scans.size(); i++)
// {
// float current_dist = base_scans[i].dist_from(new_base);
// if( current_dist < min_dist )
// {
// min_index = i;
// min_dist = current_dist;
// }
// }
// if(min_dist < REMATCH_CANDIDATE_THRESH)
// {
// boost::shared_ptr<vector<line> > temp_boost_scan(new vector<line>);
// for(int i=0; i<base_scans[min_index].lines.size(); i++)
// {
// temp_boost_scan->push_back(base_scans[min_index].lines[i]);
// }
// int expected_rot_difference = new_base.yaw -
//base_scans[min_index].yaw;
// int actual_rot_difference;
// Vector2f expected_translation_difference;
// expected_translation_difference << new_base.x -
//base_scans[min_index].x, new_base.y - base_scans[min_index].y;
// Vector2f actual_translation_difference;
// compare_scans(temp_boost_scan, scan2,
// new_base.yaw - base_scans[min_index].yaw,
//actual_rot_difference,
// expected_translation_difference,
//actual_translation_difference );
// float translation_error =
// pow(pow(expected_translation_difference(0) -
// actual_translation_difference(0), 2) +
// pow(expected_translation_difference(1) -
// actual_translation_difference(1),2), 0.5);
// if( (translation_error < REMATCH_VALIDATION_THRESH) &&
//(abs(expected_rot_difference - actual_rot_difference) < 7))
// {
// //overwrite the new_base values with the difference
//between the waypoint and the current scan
// new_base.x = base_scans[min_index].x +
//actual_translation_difference[0];
// new_base.y = base_scans[min_index].y +
//actual_translation_difference[1];
//; // new_base.yaw = base_scans[min_index].yaw + actual_rot_difference;
// }
// }
scan1 = scan2;
base_translation << new_base.x, new_base.y;
base_rot = new_base.yaw;
base_scans.push_back(new_base);
est_translation << 0, 0;
est_rot = 0;
}
previous_translation = est_translation;
previous_est_rot = est_rot;
}
geometry_msgs::Point32 pos_msg;
pos_msg.x = est_translation(0);
pos_msg.y = est_translation(1);
pos_msg.z = est_rot;
pub_pos.publish(pos_msg);
pos_msg_old.x = pos_msg.x;
pos_msg_old.y = pos_msg.y;
pos_msg_old.z = pos_msg.z;
}
else {
cout << endl << "MISSED SCAN" << endl;
pub_pos.publish(pos_msg_old);
}
}
inline std::vector<arma::vec> const &ProblemData::getSamples() const
{
return *ptrSamples.get();
}
// this makes sure we can store our connections in a set
inline bool operator<(boost::shared_ptr<receiver> const& lhs,
boost::shared_ptr<receiver> const& rhs)
{
return lhs.get() < rhs.get();
}
boost::shared_ptr<HierarchyDataOpsReal<double> >
HierarchyDataOpsManager::getOperationsDouble(
const boost::shared_ptr<hier::Variable>& variable,
const boost::shared_ptr<hier::PatchHierarchy>& hierarchy,
bool get_unique)
{
TBOX_ASSERT(variable);
TBOX_ASSERT(hierarchy);
TBOX_ASSERT_OBJDIM_EQUALITY2(*variable, *hierarchy);
const boost::shared_ptr<pdat::CellVariable<double> > cellvar(
boost::dynamic_pointer_cast<pdat::CellVariable<double>,
hier::Variable>(variable));
const boost::shared_ptr<pdat::FaceVariable<double> > facevar(
boost::dynamic_pointer_cast<pdat::FaceVariable<double>,
hier::Variable>(variable));
const boost::shared_ptr<pdat::NodeVariable<double> > nodevar(
boost::dynamic_pointer_cast<pdat::NodeVariable<double>,
hier::Variable>(variable));
const boost::shared_ptr<pdat::SideVariable<double> > sidevar(
boost::dynamic_pointer_cast<pdat::SideVariable<double>,
hier::Variable>(variable));
const boost::shared_ptr<pdat::EdgeVariable<double> > edgevar(
boost::dynamic_pointer_cast<pdat::EdgeVariable<double>,
hier::Variable>(variable));
boost::shared_ptr<HierarchyDataOpsReal<double> > ops;
if (cellvar) {
if (get_unique) {
ops.reset(new HierarchyCellDataOpsReal<double>(hierarchy));
} else {
const int n = static_cast<int>(d_cell_ops_double.size());
for (int i = 0; i < n && !ops; ++i) {
if (hierarchy !=
d_cell_ops_double[i]->getPatchHierarchy()) continue;
// A compatible operator has been found at i.
ops = d_cell_ops_double[i];
}
if (!ops) {
// No compatible operator has been found.
ops.reset(new HierarchyCellDataOpsReal<double>(hierarchy));
d_cell_ops_double.resize(n + 1);
d_cell_ops_double[n] = ops;
}
}
} else if (facevar) {
if (get_unique) {
ops.reset(new HierarchyFaceDataOpsReal<double>(hierarchy));
} else {
const int n = static_cast<int>(d_face_ops_double.size());
for (int i = 0; i < n && !ops; ++i) {
if (hierarchy !=
d_face_ops_double[i]->getPatchHierarchy()) continue;
// A compatible operator has been found at i.
ops = d_face_ops_double[i];
}
if (!ops) {
// No compatible operator has been found.
ops.reset(new HierarchyFaceDataOpsReal<double>(hierarchy));
d_face_ops_double.resize(n + 1);
d_face_ops_double[n] = ops;
}
}
} else if (nodevar) {
if (get_unique) {
ops.reset(new HierarchyNodeDataOpsReal<double>(hierarchy));
} else {
const int n = static_cast<int>(d_node_ops_double.size());
for (int i = 0; i < n && !ops; ++i) {
if (hierarchy !=
d_node_ops_double[i]->getPatchHierarchy()) continue;
// A compatible operator has been found at i.
ops = d_node_ops_double[i];
}
if (!ops) {
// No compatible operator has been found.
ops.reset(new HierarchyNodeDataOpsReal<double>(hierarchy));
d_node_ops_double.resize(n + 1);
d_node_ops_double[n] = ops;
}
}
} else if (sidevar) {
if (get_unique) {
ops.reset(new HierarchySideDataOpsReal<double>(hierarchy));
} else {
const int n = static_cast<int>(d_side_ops_double.size());
for (int i = 0; i < n && !ops; ++i) {
if (hierarchy !=
d_side_ops_double[i]->getPatchHierarchy()) continue;
// A compatible operator has been found at i.
ops = d_side_ops_double[i];
}
if (!ops) {
// No compatible operator has been found.
ops.reset(new HierarchySideDataOpsReal<double>(hierarchy));
d_side_ops_double.resize(n + 1);
d_side_ops_double[n] = ops;
}
}
} else if (edgevar) {
if (get_unique) {
ops.reset(new HierarchyEdgeDataOpsReal<double>(hierarchy));
} else {
const int n = static_cast<int>(d_edge_ops_double.size());
for (int i = 0; i < n && !ops; ++i) {
if (hierarchy !=
d_edge_ops_double[i]->getPatchHierarchy()) continue;
// A compatible operator has been found at i.
ops = d_edge_ops_double[i];
}
if (!ops) {
// No compatible operator has been found.
ops.reset(new HierarchyEdgeDataOpsReal<double>(hierarchy));
d_edge_ops_double.resize(n + 1);
d_edge_ops_double[n] = ops;
}
}
}
if (!ops) {
TBOX_ERROR("HierarchyDataOpsManager internal error...\n"
<< "Operations for variable " << variable->getName()
<< " not defined.");
}
return ops;
}
void YOGLoginScreen::handleYOGClientEvent(boost::shared_ptr<YOGClientEvent> event)
{
//std::cout<<"YOGLoginScreen: recieved event "<<event->format()<<std::endl;
Uint8 type = event->getEventType();
if(type == YEConnected)
{
attemptLogin();
}
else if(type == YEConnectionLost)
{
//shared_ptr<YOGConnectionLostEvent> info = static_pointer_cast<YOGConnectionLostEvent>(event);
animation->visible=false;
statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_LOST]"));
}
else if(type == YELoginAccepted)
{
//shared_ptr<YOGLoginAcceptedEvent> info = static_pointer_cast<YOGLoginAcceptedEvent>(event);
animation->visible=false;
runLobby();
}
else if(type == YELoginRefused)
{
shared_ptr<YOGLoginRefusedEvent> info = static_pointer_cast<YOGLoginRefusedEvent>(event);
animation->visible=false;
YOGLoginState reason = info->getReason();
if(reason == YOGPasswordIncorrect)
{
statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_BAD_PASSWORD]"));
}
else if(reason == YOGUsernameAlreadyUsed)
{
statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_ALREADY_PASSWORD]"));
}
else if(reason == YOGUserNotRegistered)
{
statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_BAD_PASSWORD_NON_ZERO]"));
}
else if(reason == YOGClientVersionTooOld)
{
statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_PROTOCOL_TOO_OLD]"));
}
else if(reason == YOGAlreadyAuthenticated)
{
statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_USERNAME_ALLREADY_USED]"));
}
else if(reason == YOGUsernameBanned)
{
statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_USERNAME_BANNED]"));
}
else if(reason == YOGIPAddressBanned)
{
statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_IP_TEMPORARILY_BANNED]"));
}
else if(reason == YOGNameInvalidSpecialCharacters)
{
statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_USERNAME_INVALID_SPECIAL_CHARACTERS]"));
}
else if(reason == YOGLoginUnknown)
{
statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_UNEXPLAINED]"));
}
client->disconnect();
}
}
void _modifyTests(SplitLongLinearWaysVisitor& splitVisitor)
{
int wayID = 0;
int startNode = 234;
int numNodes;
int numWays;
// N + 1 nodes: 2 ways
_map.reset(new OsmMap());
numNodes = splitVisitor.getMaxNumberOfNodes() + 1;
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
WayMap ways = _map->getWays();
CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// N + 2 nodes: 2 ways
_map.reset(new OsmMap());
numNodes = splitVisitor.getMaxNumberOfNodes() + 2;
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
ways = _map->getWays();
CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// (2N) - 2 nodes: 2 ways
_map.reset(new OsmMap());
numNodes = (splitVisitor.getMaxNumberOfNodes() * 2) - 2;
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
ways = _map->getWays();
CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// (2N) - 1 nodes: 2 ways
_map.reset(new OsmMap());
numNodes = (splitVisitor.getMaxNumberOfNodes() * 2) - 1;
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
ways = _map->getWays();
CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// (2N) nodes: 2 ways
_map.reset(new OsmMap());
numNodes = (splitVisitor.getMaxNumberOfNodes() * 2);
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
ways = _map->getWays();
CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// (2N) + 1 nodes: 3 ways
_map.reset(new OsmMap());
numNodes = (splitVisitor.getMaxNumberOfNodes() * 2) + 1;
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
ways = _map->getWays();
CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// (2N) + 2 nodes: 3 ways
_map.reset(new OsmMap());
numNodes = (splitVisitor.getMaxNumberOfNodes() * 2) + 2;
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
ways = _map->getWays();
CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// Picking 541N as 541 is large enough to be a good stress test, plus it's a prime number
// and primes are good at exposing problems
// (541N) - 2 nodes: 541 ways
_map.reset(new OsmMap());
numNodes = (splitVisitor.getMaxNumberOfNodes() * 541) - 2;
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// (541N) - 1 nodes: 541 ways
_map.reset(new OsmMap());
numNodes = (splitVisitor.getMaxNumberOfNodes() * 541) - 1;
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// (541N) nodes: 541 ways
_map.reset(new OsmMap());
numNodes = (splitVisitor.getMaxNumberOfNodes() * 541);
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// (541N) + 1 nodes: 542 ways
_map.reset(new OsmMap());
numNodes = (splitVisitor.getMaxNumberOfNodes() * 541) + 1;
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
// (541N) + 2 nodes: 542 ways
_map.reset(new OsmMap());
numNodes = (splitVisitor.getMaxNumberOfNodes() * 541) + 2;
numWays = _calcNumWays(numNodes, splitVisitor);
_createWay(++wayID, startNode, startNode + numNodes - 1);
_map->visitRw(splitVisitor);
// Sanity checks on split
_sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
startNode += numNodes;
}
void robox_parameters_load(robox_parameters & params,
boost::shared_ptr<sfl::OptionDictionary> opt)
{
expo_parameters_load(params, opt);
string_to(opt->GetOption("front_nscans"), params.front_nscans);
string_to(opt->GetOption("front_mount_x"), params.front_mount_x);
string_to(opt->GetOption("front_mount_y"), params.front_mount_y);
string_to(opt->GetOption("front_mount_theta"), params.front_mount_theta);
string_to(opt->GetOption("front_rhomax"), params.front_rhomax);
string_to(opt->GetOption("front_phi0"), params.front_phi0);
string_to(opt->GetOption("front_phirange"), params.front_phirange);
string_to(opt->GetOption("rear_nscans"), params.rear_nscans);
string_to(opt->GetOption("rear_mount_x"), params.rear_mount_x);
string_to(opt->GetOption("rear_mount_y"), params.rear_mount_y);
string_to(opt->GetOption("rear_mount_theta"), params.rear_mount_theta);
string_to(opt->GetOption("rear_rhomax"), params.rear_rhomax);
string_to(opt->GetOption("rear_phi0"), params.rear_phi0);
string_to(opt->GetOption("rear_phirange"), params.rear_phirange);
}