/* callback with smoothed normals
*
* Note that the pointers are to GPU memory as indicated by the "d_" prefix.
*/
virtual void normals_cb(float* d_normalsImg, uint8_t* d_haveData, uint32_t w, uint32_t h)
{
if(w==0 || h==0) return;
// pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr nDispPtr =
// normalExtract->normalsPc();
boost::mutex::scoped_lock updateLock(updateModelMutex);
// nDisp_ = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(
// new pcl::PointCloud<pcl::PointXYZRGB>(*nDispPtr));
normalsImg_ = normalExtract->normalsImg();
if(false)
{
static int frameN = 0;
if(frameN==0) if(system("mkdir ./normals/") >0){
cout<<"problem creating subfolder for results"<<endl;
};
char path[100];
// Save the image data in binary format
sprintf(path,"./normals/%05d.bin",frameN ++);
if(compress_)
{
int nComp;
normalsComp_ = normalExtract->normalsComp(nComp);
imwriteBinary(std::string(path), normalsComp_);
}else
imwriteBinary(std::string(path), normalsImg_);
}
this->update_ = true;
};
void OpenniVisualizer::visualizerThread()
{
#ifdef USE_PCL_VIEWER
if (visualizeCloud_) {
// prepare visualizer named "viewer"
viewer_ = boost::shared_ptr<pcl::visualization::PCLVisualizer>(
new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer_->initCameraParameters ();
viewer_->setBackgroundColor (255, 255, 255);
viewer_->addCoordinateSystem (1.0);
viewer_->setSize(1000,1000);
while (!viewer_->wasStopped ()) {
viewer_->spinOnce (10);
char key = cv::waitKey(10);
// Get lock on the boolean update and check if cloud was updated
boost::mutex::scoped_lock updateLock(updateModelMutex);
if (update_) {
visualize_(key);
update_ = false;
}
}
} else {
while (42) {
char key = cv::waitKey(10);
// Get lock on the boolean update and check if cloud was updated
boost::mutex::scoped_lock updateLock(updateModelMutex);
if (update_) {
visualize_(key);
update_ = false;
}
}
}
#else
while (42)
{
char key = cv::waitKey(10);
// Get lock on the boolean update and check if cloud was updated
boost::mutex::scoped_lock updateLock(updateModelMutex);
if (update_)
{
visualize_(key);
update_ = false;
}
}
#endif
}
void ThreadSafeSinglePropertyLookupCache::updateGet(MonoClass* dest_class, const char* message, MonoObject* args[], int nargs, MonoMethod* resolved) {
boost::mutex::scoped_lock updateLock(mMutex, boost::try_to_lock_t());
if (!updateLock.owns_lock())
return;
mDestClass = dest_class;
mResolvedGetMethod = resolved;
mResolvedSetMethod = NULL;
}
virtual void depth_cb(const uint16_t * depth, uint32_t w, uint32_t h)
{
cv::Mat dMap = cv::Mat(h,w,CV_16U,const_cast<uint16_t*>(depth));
{
boost::mutex::scoped_lock updateLock(updateModelMutex);
dColor_ = colorizeDepth(dMap, 30.,4000.);
d_ = dMap.clone();
}
this->update();
};
PFilterTest::PFilterTest(int n_particles, particleFilter::cspace b_init[2]) :
pFilter_(n_particles, b_init, 0.001, 0.0035, 0.0001, 0.001),
num_voxels{200, 200, 200}//,
//dist_transform(num_voxels)
// particleFilter (int n_particles, cspace b_init[2],
// double Xstd_ob=0.0001 (measurement error),
// double Xstd_tran=0.0025, (gausian kernel sampling std
// double Xstd_scatter=0.0001, (scatter particles a little before computing mean of distance transform
// double R=0.01) (probe radius);
{
// sub_init = n.subscribe("/particle_filter_init", 1, &PFilterTest::initDistribution, this);
srv_add_obs = n.advertiseService("/particle_filter_add", &PFilterTest::addObs, this);
pub_particles = n.advertise<geometry_msgs::PoseArray>("/particles_from_filter", 5);
ROS_INFO("Loading Boeing Particle Filter");
getMesh("boeing_part.stl");
//int num_voxels[3] = { 200,200,200 };
//dist_transform(num_voxels);
ROS_INFO("start create dist_transform");
dist_transform = new distanceTransform(num_voxels);
#ifdef POINT_CLOUD
particleFilter::cspace particles[NUM_PARTICLES];
pFilter_.getAllParticles(particles);
boost::mutex::scoped_lock updateLock(updateModelMutex);
basic_cloud_ptr1->points.clear();
basic_cloud_ptr2->points.clear();
for (int j = 0; j < NUM_PARTICLES; j++ ) {
pcl::PointXYZ basic_point;
basic_point.x = particles[j][0] * 2;
basic_point.y = particles[j][1] * 2;
basic_point.z = particles[j][2] * 2;
basic_cloud_ptr1->points.push_back(basic_point);
basic_point.x = particles[j][3] * 2;
basic_point.y = particles[j][4] * 2;
basic_point.z = particles[j][5] * 2;
basic_cloud_ptr2->points.push_back(basic_point);
}
basic_cloud_ptr1->width = (int) basic_cloud_ptr1->points.size ();
basic_cloud_ptr1->height = 1;
basic_cloud_ptr2->width = (int) basic_cloud_ptr2->points.size ();
basic_cloud_ptr2->height = 1;
update = true;
updateLock.unlock();
#endif
ros::Duration(1.0).sleep();
pub_particles.publish(getParticlePoseArray());
}
virtual void rgb_cb_ (const boost::shared_ptr<openni_wrapper::Image>& rgb)
{
// overwrite to use the lock to update rgb;
int w = rgb->getWidth();
int h = rgb->getHeight();
// TODO: uggly .. but avoids double copy of the image.
boost::mutex::scoped_lock updateLock(updateModelMutex);
if(this->rgb_.cols < w) {
this->rgb_ = cv::Mat(h,w,CV_8UC3);
}
cv::Mat Irgb(h,w,CV_8UC3);
rgb->fillRGB(w,h,Irgb.data);
cv::cvtColor(Irgb, this->rgb_,CV_BGR2RGB);
rgb_cb(this->rgb_.data,w,h);
}
geometry_msgs::PoseArray PFilterTest::getParticlePoseArray(int idx)
{
std::vector<cspace> particles;
pFilter_.getAllParticles(particles, idx);
// tf::Transform trans = pHandler.getTransformToPartFrame();
tf::Transform trans = trans_;
if (idx == 5) trans = trans1_;
#ifdef POINT_CLOUD
boost::mutex::scoped_lock updateLock(updateModelMutex);
basic_cloud_ptr1->points.clear();
basic_cloud_ptr2->points.clear();
int numParticles = pFilter_.getNumParticles();
for (int j = 0; j < numParticles; j++ ) {
pcl::PointXYZ basic_point;
basic_point.x = particles[j][0] * 2;
basic_point.y = particles[j][1] * 2;
basic_point.z = particles[j][2] * 2;
basic_cloud_ptr1->points.push_back(basic_point);
basic_point.x = particles[j][3] * 2;
basic_point.y = particles[j][4] * 2;
basic_point.z = particles[j][5] * 2;
basic_cloud_ptr2->points.push_back(basic_point);
}
basic_cloud_ptr1->width = (int) basic_cloud_ptr1->points.size ();
basic_cloud_ptr1->height = 1;
basic_cloud_ptr2->width = (int) basic_cloud_ptr2->points.size ();
basic_cloud_ptr2->height = 1;
update = true;
updateLock.unlock();
#endif
// cspace particles_est_stat;
// cspace particles_est;
// pFilter_.estimateGaussian(particles_est, particles_est_stat);
geometry_msgs::PoseArray poseArray;
for(int i=0; i<50; i++){
tf::Pose pose = poseAt(particles[i]);
geometry_msgs::Pose pose_msg;
tf::poseTFToMsg(trans*pose, pose_msg);
poseArray.poses.push_back(pose_msg);
// ROS_INFO("Pose %d: %f, %f, %f", i, poseArray.poses[i].position.x,
// poseArray.poses[i].position.y,
// poseArray.poses[i].position.z);
}
return poseArray;
}
geometry_msgs::PoseArray PFilterTest::getParticlePoseArray()
{
particleFilter::cspace particles[NUM_PARTICLES];
pFilter_.getAllParticles(particles);
tf::Transform trans = plt.getTrans();
#ifdef POINT_CLOUD
boost::mutex::scoped_lock updateLock(updateModelMutex);
basic_cloud_ptr1->points.clear();
basic_cloud_ptr2->points.clear();
for (int j = 0; j < pFilter_.numParticles; j++ ) {
pcl::PointXYZ basic_point;
basic_point.x = particles[j][0] * 2;
basic_point.y = particles[j][1] * 2;
basic_point.z = particles[j][2] * 2;
basic_cloud_ptr1->points.push_back(basic_point);
basic_point.x = particles[j][3] * 2;
basic_point.y = particles[j][4] * 2;
basic_point.z = particles[j][5] * 2;
basic_cloud_ptr2->points.push_back(basic_point);
}
basic_cloud_ptr1->width = (int) basic_cloud_ptr1->points.size ();
basic_cloud_ptr1->height = 1;
basic_cloud_ptr2->width = (int) basic_cloud_ptr2->points.size ();
basic_cloud_ptr2->height = 1;
update = true;
updateLock.unlock();
#endif
particleFilter::cspace particles_est_stat;
particleFilter::cspace particles_est;
pFilter_.estimatedDistribution(particles_est, particles_est_stat);
geometry_msgs::PoseArray poseArray;
for(int i=0; i<50; i++){
tf::Pose pose = poseAt(particles[i]);
geometry_msgs::Pose pose_msg;
tf::poseTFToMsg(trans*pose, pose_msg);
poseArray.poses.push_back(pose_msg);
// ROS_INFO("Pose %d: %f, %f, %f", i, poseArray.poses[i].position.x,
// poseArray.poses[i].position.y,
// poseArray.poses[i].position.z);
}
return poseArray;
}
/*
* Visualize particles
*/
void visualize()
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->initCameraParameters ();
int v1 = 0;
viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
viewer->setBackgroundColor (0, 0, 0, v1);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,1,1, "sample cloud1", v1);
viewer->addText("x y z", 15, 120, 20, 1, 1, 1, "v1 text", v1);
int v2 = 1;
viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
viewer->setBackgroundColor (0, 0, 0, v2);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,1,1, "sample cloud2", v2);
viewer->addText("roll pitch yaw", 15, 120, 20, 1, 1, 1, "v2 text", v2);
viewer->addCoordinateSystem (1.0);
try {
while (!viewer->wasStopped ())
{
boost::mutex::scoped_lock updateLock(updateModelMutex);
if(update)
{
if(!viewer->updatePointCloud<pcl::PointXYZ>(basic_cloud_ptr1, "sample cloud1"))
viewer->addPointCloud<pcl::PointXYZ>(basic_cloud_ptr1, "sample cloud1", v1);
if(!viewer->updatePointCloud<pcl::PointXYZ>(basic_cloud_ptr2, "sample cloud2"))
viewer->addPointCloud<pcl::PointXYZ>(basic_cloud_ptr2, "sample cloud2", v2);
update = false;
}
updateLock.unlock();
viewer->spinOnce (100);
}
} catch(boost::thread_interrupted&)
{
viewer->close();
return;
}
}
void update(){
boost::mutex::scoped_lock updateLock(updateModelMutex);
update_=true;
};
void DIALOG_SPICE_MODEL::loadLibrary( const wxString& aFilePath )
{
wxString curModel = m_modelName->GetValue();
m_models.clear();
wxFileName filePath( aFilePath );
bool in_subckt = false; // flag indicating that the parser is inside a .subckt section
// Look for the file in the project path
if( !filePath.Exists() )
{
filePath.SetPath( Prj().GetProjectPath() + filePath.GetPath() );
if( !filePath.Exists() )
return;
}
// Display the library contents
wxWindowUpdateLocker updateLock( this );
m_libraryContents->Clear();
wxTextFile file;
file.Open( filePath.GetFullPath() );
int line_nr = 0;
// Process the file, looking for components
while( !file.Eof() )
{
const wxString& line = line_nr == 0 ? file.GetFirstLine() : file.GetNextLine();
m_libraryContents->AppendText( line );
m_libraryContents->AppendText( "\n" );
wxStringTokenizer tokenizer( line );
while( tokenizer.HasMoreTokens() )
{
wxString token = tokenizer.GetNextToken().Lower();
// some subckts contain .model clauses inside,
// skip them as they are a part of the subckt, not another model
if( token == ".model" && !in_subckt )
{
wxString name = tokenizer.GetNextToken();
if( name.IsEmpty() )
break;
token = tokenizer.GetNextToken();
SPICE_PRIMITIVE type = MODEL::parseModelType( token );
if( type != SP_UNKNOWN )
m_models.emplace( name, MODEL( line_nr, type ) );
}
else if( token == ".subckt" )
{
wxASSERT( !in_subckt );
in_subckt = true;
wxString name = tokenizer.GetNextToken();
if( name.IsEmpty() )
break;
m_models.emplace( name, MODEL( line_nr, SP_SUBCKT ) );
}
else if( token == ".ends" )
{
wxASSERT( in_subckt );
in_subckt = false;
}
}
++line_nr;
}
wxArrayString modelsList;
// Refresh the model name combobox values
m_modelName->Clear();
for( const auto& model : m_models )
{
m_modelName->Append( model.first );
modelsList.Add( model.first );
}
m_modelName->AutoComplete( modelsList );
// Restore the previous value or if there is none - pick the first one from the loaded library
if( !curModel.IsEmpty() )
m_modelName->SetValue( curModel );
else if( m_modelName->GetCount() > 0 )
m_modelName->SetSelection( 0 );
}
PFilterTest::PFilterTest(int n_particles, cspace b_init[2]) :
pFilter_(n_particles, b_init, 0.0005, 0.000),
num_voxels{200, 200, 200}//,
// pFilter_(n_particles, b_init, 0.001, 0.0025, 0.0001, 0.00),
// num_voxels{300, 300, 300}//,
//dist_transform(num_voxels)
// particleFilter (int n_particles, cspace b_init[2],
// double Xstd_ob=0.0001 (measurement error),
// double Xstd_tran=0.0025, (gausian kernel sampling std
// double Xstd_scatter=0.0001, (scatter particles a little before computing mean of distance transform
// double R=0.01) (probe radius);
{
if(!n.getParam("localization_object", cadName)){
ROS_INFO("Failed to get param: localization_object");
}
// sub_init = n.subscribe("/particle_filter_init", 1, &PFilterTest::initDistribution, this);
sub_request_particles = n.subscribe("/request_particles", 1, &PFilterTest::sendParticles, this);
srv_add_obs = n.advertiseService("/particle_filter_add", &PFilterTest::addObs, this);
pub_particles = n.advertise<geometry_msgs::PoseArray>("/particles_from_filter", 5);
pub_particles1 = n.advertise<geometry_msgs::PoseArray>("/right_datum/particles_from_filter", 5);
pub_particles2 = n.advertise<geometry_msgs::PoseArray>("/left_datum/particles_from_filter", 5);
ROS_INFO("Loading Boeing Particle Filter");
// getMesh("boeing_part.stl");
getMesh("wood_boeing.stl");
// getMesh("part.stl");
//int num_voxels[3] = { 200,200,200 };
//dist_transform(num_voxels);
ROS_INFO("start create dist_transform");
dist_transform = new distanceTransform(num_voxels);
tf::TransformListener tf_listener_;
tf_listener_.waitForTransform("/my_frame", "wood_boeing", ros::Time(0), ros::Duration(10.0));
tf_listener_.lookupTransform("wood_boeing", "/my_frame", ros::Time(0), trans_);
tf_listener_.waitForTransform("/my_frame", "right_datum", ros::Time(0), ros::Duration(10.0));
tf_listener_.lookupTransform("right_datum", "/my_frame", ros::Time(0), trans1_);
ROS_INFO("finish create dist_transform");
#ifdef POINT_CLOUD
std::vector<cspace> particles;
pFilter_.getAllParticles(particles, 0);
boost::mutex::scoped_lock updateLock(updateModelMutex);
basic_cloud_ptr1->points.clear();
basic_cloud_ptr2->points.clear();
for (int j = 0; j < NUM_PARTICLES; j++ ) {
pcl::PointXYZ basic_point;
basic_point.x = particles[j][0] * 2;
basic_point.y = particles[j][1] * 2;
basic_point.z = particles[j][2] * 2;
basic_cloud_ptr1->points.push_back(basic_point);
basic_point.x = particles[j][3] * 2;
basic_point.y = particles[j][4] * 2;
basic_point.z = particles[j][5] * 2;
basic_cloud_ptr2->points.push_back(basic_point);
}
basic_cloud_ptr1->width = (int) basic_cloud_ptr1->points.size ();
basic_cloud_ptr1->height = 1;
basic_cloud_ptr2->width = (int) basic_cloud_ptr2->points.size ();
basic_cloud_ptr2->height = 1;
update = true;
updateLock.unlock();
#endif
ros::Duration(1.0).sleep();
pub_particles.publish(getParticlePoseArray(0));
pub_particles1.publish(getParticlePoseArray(5));
pub_particles2.publish(getParticlePoseArray(6));
}
void CamSettingsPage::UpdateWidgets()
{
Glib::Mutex::Lock updateLock( m_updateWidgetMutex, Glib::NOT_LOCK );
if ( updateLock.try_acquire() != true )
{
return;
}
if ( m_pCamera == NULL || IsConnected() != true )
{
return;
}
UpdateCameraPowerCheckButton();
if ( m_absMode != m_pCheckbuttonAbsMode->get_active() )
{
m_pCheckbuttonAbsMode->set_active( m_absMode );
}
for ( unsigned int i = 0; i < sk_numProps; i++ )
{
if ( m_pCamera == 0 )
{
return;
}
Gtk::CheckButton* pAuto = m_widgetPropArray[i].pAuto;
Gtk::CheckButton* pOnOff = m_widgetPropArray[i].pOnOff;
Gtk::CheckButton* pOnePush = m_widgetPropArray[i].pOnePush;
Gtk::HScale* pHScale1 = m_widgetPropArray[i].pHScale1;
Gtk::HScale* pHScale2 = m_widgetPropArray[i].pHScale2;
Gtk::SpinButton* pSpinButton1 = m_widgetPropArray[i].pSpinButton1;
Gtk::SpinButton* pSpinButton2 = m_widgetPropArray[i].pSpinButton2;
Gtk::Adjustment* pAdjustment1 = m_widgetPropArray[i].pAdjustment1;
Gtk::Adjustment* pAdjustment2 = m_widgetPropArray[i].pAdjustment2;
Gtk::Label* pLabel1 = m_widgetPropArray[i].pLabel1;
Gtk::Label* pLabel2 = m_widgetPropArray[i].pLabel2;
const PropertyType k_currPropType = (PropertyType)i;
Property camProp;
PropertyInfo camPropInfo;
// Get the property and property info
camProp.type = k_currPropType;
camPropInfo.type = k_currPropType;
Error getPropErr = m_pCamera->GetProperty( &camProp );
Error getPropInfoErr = m_pCamera->GetPropertyInfo( &camPropInfo );
if ( getPropErr != PGRERROR_OK ||
getPropInfoErr != PGRERROR_OK ||
camPropInfo.present == false)
{
// Perhaps not supported, hide it and continue
HideProperty( k_currPropType );
continue;
}
if ( pAuto != 0 )
{
UpdateAutoCheckButton( pAuto, camPropInfo.autoSupported, camProp.autoManualMode );
}
if ( pOnOff != 0 )
{
UpdateOnOffCheckButton( pOnOff, camPropInfo.onOffSupported, camProp.onOff );
}
if ( pOnePush != 0 )
{
UpdateOnePushCheckButton( pOnePush, camPropInfo.onePushSupported, camProp.onePush );
}
if ( pAdjustment1 != 0 )
{
UpdateAdjustment( pAdjustment1, &camPropInfo, &camProp );
pHScale1->show();
pHScale1->set_sensitive(true);
UpdateSpinButton( pSpinButton1, &camPropInfo );
pSpinButton1->show();
}
if ( pAdjustment2 != 0 )
{
UpdateAdjustment( pAdjustment2, &camPropInfo, &camProp );
pHScale2->show();
pHScale2->set_sensitive(true);
UpdateSpinButton( pSpinButton2, &camPropInfo );
pSpinButton2->show();
}
if ( pLabel1 != 0 )
{
UpdateLabel(pLabel1, &camPropInfo, &camProp );
}
if ( pLabel2 != 0 )
{
UpdateLabel(pLabel2, &camPropInfo, &camProp );
}
}
}
