void RobotState::DVLCallback(const underwater_sensor_msgs::DVLConstPtr &message)
{
cv::Point3f white_noise(var_nor(),var_nor(),var_nor());
float elapsed_time;
if(message->bi_error < 1)
{
vel_dvl_.x = message->bi_x_axis;
vel_dvl_.y = message->bi_y_axis;
vel_dvl_.z = message->bi_z_axis;
//vel_dvl_ += white_noise;
//ROS_DEBUG_STREAM_NAMED("rs","DVL velocity = " << vel_dvl_);
if(has_dvl_ && has_imu_)
{
elapsed_time = (message->header.stamp - timestamp_).toSec();
computeTraveledDistances(elapsed_time);
}
timestamp_ = message->header.stamp;
has_dvl_ = true;
}
else
{
ROS_DEBUG_STREAM_NAMED("rs","O erro da DVL eh " << message->bi_error);
}
}
double rnorm(double mean, double sd, boost::mt19937& rng) {
boost::normal_distribution<> nd(mean, sd);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng, nd);
double val = -1.0;
while(val < 0) {
val = var_nor();
}
return val;
}
void initLight(){
boost::mt19937 rng; // I don't seed it on purpouse (it's not relevant)
boost::normal_distribution<> nd(0.0, 1.0);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng, nd);
for (int i=0; i < 1024; ++i){
_aLight[i] = float(abs(var_nor()));
}
}
osg::Vec3d GPSSensor::getMeasurement() {
//Should get world coords and then transform to the localizedWorld
osg::Matrixd *rMs=getWorldCoords(node_);
osg::Matrixd lMs=*rMs*osg::Matrixd::inverse(rMl_);
//Now add some gaussian noise
static boost::normal_distribution<> normal(0,std_);
static boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng_, normal);
return lMs.getTrans()+osg::Vec3d(var_nor(), var_nor(), var_nor());
}
double Controller::randn(double mean, double stdev) {
// static random_device e { };
// static normal_distribution<double> d(mean, stdev);
// return d(e);
boost::mt19937 rng; // I don't seed it on purpouse (it's not relevant)
rng.seed((++seed) + time(NULL));
boost::normal_distribution<> nd(mean, stdev);
boost::variate_generator<boost::mt19937&,
boost::normal_distribution<> > var_nor(rng, nd);
return var_nor();
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboTreasure::Update()
{
if ( static_object_ || terminated_ ) {
return;
}
// produces randomness out of thin air
static boost::random::mt19937 rng;
// distribution that maps to 0..9
static boost::random::uniform_int_distribution<> change_direction(0,299);
// normal distribution
static boost::normal_distribution<> nd(0.0, 1.0);
static boost::variate_generator<boost::mt19937&,
boost::normal_distribution<> > var_nor(rng, nd);
// max 5km/h = 1.38889 = 5 km/h
if ( change_direction(rng) == 0 )
{
vel_x += var_nor();
vel_y += var_nor();
vel_yaw += var_nor();
double vel = sqrt((vel_x*vel_x)+(vel_y*vel_y));
if ( vel > 1.3889 ) {
vel_x *= 1.3389/vel;
vel_y *= 1.3389/vel;
}
if (fabs(vel_yaw) > 1.3389) {
vel_yaw *= 1.3389/fabs(vel_yaw);
}
}
math::Pose pose = link_->GetWorldPose();
// 100 x 60
if ( pose.pos.x > 45 ) {
vel_x = -fabs(vel_x);
}
if ( pose.pos.x < -45 ) {
vel_x = fabs(vel_x);
}
if ( pose.pos.y > 25 ) {
vel_y = -fabs(vel_y);
}
if ( pose.pos.y < -25 ) {
vel_y = fabs(vel_y);
}
model_->SetLinearVel(math::Vector3(vel_x, vel_y, 0));
model_->SetAngularVel(math::Vector3(0, 0, vel_yaw));
last_time_ = world_->GetSimTime();
// check score
}
pcl::PointCloud<pcl::PointXYZ> generateData(int data) {
//normal distribution
boost::mt19937 rng;
rng.seed(clock());
boost::normal_distribution<> nd(0, 0.1*data);
boost::variate_generator<boost::mt19937&,
boost::normal_distribution<> > var_nor(rng, nd);
pcl::PointCloud<pcl::PointXYZ> pc;
for(int i=0; i<100; i++) {
pcl::PointXYZ p;
p.x=(i-50)/50.;
p.y=0;
p.z=1;
ADD_NOISE();
pc.push_back(p);
p.y=1;
ADD_NOISE();
pc.push_back(p);
p.z=2;
ADD_NOISE();
pc.push_back(p);
p.y=0;
ADD_NOISE();
pc.push_back(p);
}
for(int i=1; i<99; i++) {
pcl::PointXYZ p;
p.x=-1;
p.y=0;
p.z=1+i/100.;
ADD_NOISE();
pc.push_back(p);
p.x=1;
ADD_NOISE();
pc.push_back(p);
p.y=1;
ADD_NOISE();
pc.push_back(p);
p.x=-1;
ADD_NOISE();
pc.push_back(p);
}
return pc;
}
void addNoise(pcl::PointCloud<pcl::PointXYZ> &pc, int noise)
{
//normal distribution
boost::mt19937 rng;
rng.seed(clock());
boost::normal_distribution<> nd(0, 0.02*noise);
boost::variate_generator<boost::mt19937&,
boost::normal_distribution<> > var_nor(rng, nd);
for(int i=0; i<pc.size(); i++) {
pcl::PointXYZ &p = pc[i];
ADD_NOISE();
}
}
/**
* Generates sample points using a few gauissians.
* Then applies gmm fitting on these generated points.
* Provides a heuristic score for each gmm fitting result.
*
*/
int main(void)
{
boost::mt19937 engine;
engine.seed(1340818006);
// generate random data using gauissians below
std::vector< boost::normal_distribution<double> > distributions;
distributions.push_back(boost::normal_distribution<double>(0.1, 0.05));
distributions.push_back(boost::normal_distribution<double>(0.4, 0.1));
distributions.push_back(boost::normal_distribution<double>(0.55, 0.05));
distributions.push_back(boost::normal_distribution<double>(0.7, 0.1));
distributions.push_back(boost::normal_distribution<double>(0.9, 0.05));
distributions.push_back(boost::normal_distribution<double>(1.0, 0.05));
std::vector<double> data;
for(std::vector< boost::normal_distribution<double> >::iterator it = distributions.begin();
it != distributions.end(); ++it)
{
boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > var_nor(engine, *it);
for(std::size_t i = 0; i < 300; ++i) { data.push_back(var_nor()); }
}
// calculate closest center (using above gauissians) for each generated points
// we will compare it with gmm fitting results
// also we might want to compute mixing coef for each center and select centers according to mixing_coef * prob(data)
std::vector<std::size_t> data_centers;
for(std::vector<double>::iterator it = data.begin(); it != data.end(); ++it)
{
std::size_t center_id = (std::numeric_limits<std::size_t>::max)(), center_counter = 0;;
double min_distance = (std::numeric_limits<double>::max)();
for(std::vector< boost::normal_distribution<double> >::iterator dis_it = distributions.begin();
dis_it != distributions.end(); ++dis_it, ++center_counter)
{
double distance = std::abs(*it - dis_it->mean());
if(min_distance > distance)
{
min_distance = distance;
center_id = center_counter;
}
}
data_centers.push_back(center_id);
}
// apply gmm fitting clustering
typedef CGAL::internal::Expectation_maximization E_M;
std::vector<E_M> gmm_fitters;
gmm_fitters.push_back(E_M(distributions.size(), data, E_M::PLUS_INITIALIZATION));
gmm_fitters.push_back(E_M(distributions.size(), data, E_M::RANDOM_INITIALIZATION));
gmm_fitters.push_back(E_M(distributions.size(), data, E_M::K_MEANS_INITIALIZATION));
std::vector< std::vector<std::size_t> > calculated_centers(gmm_fitters.size());
std::vector< std::vector<std::size_t> >::iterator calc_centers_it = calculated_centers.begin();
for(std::vector<E_M>::iterator it = gmm_fitters.begin(); it != gmm_fitters.end(); ++it, ++calc_centers_it)
{
it->fill_with_center_ids(*calc_centers_it);
}
std::cout << "Compare results of EM with 'expected' (but be aware, it is not optimal result in terms of likelihood)" << std::endl;
std::cout << "Another words a clustering which has higher likelihood can result in worse score in here" << std::endl;
for(std::vector< std::vector<std::size_t> >::iterator calc_centers_it = calculated_centers.begin();
calc_centers_it != calculated_centers.end(); ++calc_centers_it)
{
std::size_t true_count = 0;
std::vector<std::size_t>::iterator calculated_it = calc_centers_it->begin();
for(std::vector<std::size_t>::iterator it = data_centers.begin(); it != data_centers.end(); ++it, ++calculated_it)
{
if( (*it) == (*calculated_it) ) { ++true_count; }
}
double app_fit = static_cast<double>(true_count) / data_centers.size();
std::cout << "[0,1]: " << app_fit << std::endl;
if(app_fit < 0.7) {
std::cerr << "There might be a problem if above printed comparison is too low." << std::endl;
return EXIT_FAILURE;
}
}
}
inline double gaussian(const double mean, const double sigma)
{
boost::normal_distribution<> g(mean,sigma);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(random::generator,g);
return var_nor();
}
void
DoSampleRun2 ()
{
cob_3d_mapping_filters::SpeckleFilter<PointXYZ> filter;
pcl::PointCloud<PointXYZ>::Ptr cloud (new pcl::PointCloud<PointXYZ> ());
pcl::PointCloud<PointXYZ>::Ptr cloud_out (new pcl::PointCloud<PointXYZ> ());
cloud->width = 640;
cloud->height = 480;
double x = 0, y = 0;
for (unsigned int i = 0; i < cloud->width; i++, y += 0.001)
{
x = 0;
for (unsigned int j = 0; j < cloud->height; j++, x += 0.001)
{
PointXYZ pt;
pt.x = x;
pt.y = y;
pt.z = 1;
cloud->points.push_back (pt);
}
}
boost::mt19937 rng; // I don't seed it on purpouse (it's not relevant)
boost::normal_distribution<> nd (0.0, 0.05);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
for (unsigned int i = 0; i < 3000; i++)
cloud->points[i * 100].z += var_nor ();
//pcl::io::savePCDFileASCII("/tmp/spk_cloud.pcd", *cloud);
filter.setInputCloud (cloud);
for (unsigned int s = 10; s <= 100; s += 10)
{
std::stringstream ss;
ss << "/tmp/spk_acc_" << s << ".txt";
std::ofstream file;
file.open (ss.str ().c_str ());
file << "thr\ttp\tfn\tfp\n";
for (double c = 0.01; c <= 0.1; c += 0.01)
{
filter.setFilterParam (s, c);
filter.filter (*cloud_out);
pcl::PointIndices::Ptr ind = filter.getRemovedIndices ();
std::cout << "Cloud size " << cloud_out->size () << ", ind: " << ind->indices.size () << std::endl;
int fn_ctr = 0, tp_ctr = 0;
for (unsigned int i = 0; i < 3000; i++)
{
bool found = false;
for (unsigned int j = 0; j < ind->indices.size (); j++)
{
if (ind->indices[j] == i * 100)
{
tp_ctr++;
found = true;
break;
}
}
if (!found)
fn_ctr++;
}
int fp_ctr = ind->indices.size () - tp_ctr;
double fn_ratio = (double)fn_ctr / 3000;
double fp_ratio = (double)fp_ctr / 3000;
double tp_ratio = (double)tp_ctr / 3000;
file << c << "\t" << tp_ratio << "\t" << fn_ratio << "\t" << fp_ratio << "\n";
std::cout << "c: " << c << " fn: " << fn_ratio << ", tp: " << tp_ratio << " fp: " << fp_ratio << std::endl;
}
file.close ();
}
}
void SphereGenerator::Generate() {
//generate
if (nr_of_outliers > nr_of_points)
nr_of_outliers = 0;
// Fill in the cloud data
cloud.width = nr_of_points;
cloud.height = 1;
cloud.points.resize (cloud.width * cloud.height);
// Generate the data
for (size_t i = 0; i < cloud.points.size (); ++i)
{
cloud.points[i].x = rand () / (RAND_MAX + 1.0f) * r;
if (rand()%2)
cloud.points[i].x = - cloud.points[i].x;
cloud.points[i].y = rand () / (RAND_MAX + 1.0f) * sqrt(r*r- cloud.points[i].x*cloud.points[i].x);
if (rand()%2)
cloud.points[i].y = - cloud.points[i].y;
cloud.points[i].z = sqrt(r*r - cloud.points[i].x*cloud.points[i].x - cloud.points[i].y*cloud.points[i].y);
if (rand()%2)
cloud.points[i].z = - cloud.points[i].z;
cloud.points[i].x+=x;
cloud.points[i].y+=y;
cloud.points[i].z+=z;
}
// Set a few outliers
for (int i = 0; i < nr_of_outliers; ++i){
cloud.points[i].x += (rand () / (RAND_MAX + 1.0f) * 10) -5;
cloud.points[i].y += (rand () / (RAND_MAX + 1.0f) * 10) -5;
cloud.points[i].z += (rand () / (RAND_MAX + 1.0f) * 10) -5;
}
//noise
struct timeval start;
gettimeofday (&start, NULL);
boost::mt19937 rng;
rng.seed (start.tv_usec);
boost::normal_distribution<> nd (mi, sigma);
boost::variate_generator<boost::mt19937&,
boost::normal_distribution<> > var_nor (rng, nd);
// Noisify each point in the dataset
for (size_t i = nr_of_outliers; i < cloud.points.size (); ++i)
{
cloud.points[i].x += var_nor ();
cloud.points[i].y += var_nor ();
cloud.points[i].z += var_nor ();
}
out_pcl.write(cloud);
cloudPtr = cloud.makeShared();
out_pcl_ptr.write(cloudPtr);
}
void PlaneGenerator::Generate() {
if (nr_of_outliers > nr_of_points)
nr_of_outliers = 0;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = nr_of_points;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
}
// Create a set of planar coefficients
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
coefficients->values.resize (4);
coefficients->values[0] = a;
coefficients->values[1] = b;
coefficients->values[2] = c;
coefficients->values[3] = d;
// Create the filtering object
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (cloud);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud);
// Set a few outliers
for (int i = 0; i < nr_of_outliers; ++i){
cloud->points[i].x += (rand () / (RAND_MAX + 1.0f) * 5) +1;
if (rand()%2)
cloud->points[i].x=-cloud->points[i].x;
cloud->points[i].y += (rand () / (RAND_MAX + 1.0f) * 5) +1;
if (rand()%2)
cloud->points[i].y=-cloud->points[i].y;
cloud->points[i].z += (rand () / (RAND_MAX + 1.0f) * 5) +1;
if (rand()%2)
cloud->points[i].z=-cloud->points[i].z;
}
//noise
struct timeval start;
gettimeofday (&start, NULL);
boost::mt19937 rng;
rng.seed (start.tv_usec);
boost::normal_distribution<> nd (mi, sigma);
boost::variate_generator<boost::mt19937&,
boost::normal_distribution<> > var_nor (rng, nd);
// Noisify each point in the dataset
for (size_t i = nr_of_outliers; i < cloud->points.size (); ++i)
{
cloud->points[i].x += var_nor ();
cloud->points[i].y += var_nor ();
cloud->points[i].z += var_nor ();
}
out_pcl.write(cloud);
}
void
pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::initialize (bool force_retrain)
{
//use the source to know what has to be trained and what not, checking if the descr_name directory exists
//unless force_retrain is true, then train everything
boost::shared_ptr < std::vector<ModelT> > models = source_->getModels ();
std::cout << "Models size:" << models->size () << std::endl;
if (force_retrain)
{
for (size_t i = 0; i < models->size (); i++)
{
source_->removeDescDirectory (models->at (i), training_dir_, descr_name_);
}
}
for (size_t i = 0; i < models->size (); i++)
{
if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_))
{
for (size_t v = 0; v < models->at (i).views_->size (); v++)
{
PointInTPtr processed (new pcl::PointCloud<PointInT>);
PointInTPtr view = models->at (i).views_->at (v);
if (view->points.size () == 0)
PCL_WARN("View has no points!!!\n");
if (noisify_)
{
double noise_std = noise_;
boost::posix_time::ptime time = boost::posix_time::microsec_clock::local_time();
boost::posix_time::time_duration duration( time.time_of_day() );
boost::mt19937 rng;
rng.seed (static_cast<unsigned int> (duration.total_milliseconds()));
boost::normal_distribution<> nd (0.0, noise_std);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
// Noisify each point in the dataset
for (size_t cp = 0; cp < view->points.size (); ++cp)
view->points[cp].z += static_cast<double> (var_nor ());
}
//pro view, compute signatures
std::vector<pcl::PointCloud<FeatureT>, Eigen::aligned_allocator<pcl::PointCloud<FeatureT> > > signatures;
std::vector < Eigen::Vector3d > centroids;
micvfh_estimator_->estimate (view, processed, signatures, centroids);
std::vector<bool> valid_trans;
std::vector < Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > transforms;
micvfh_estimator_->getValidTransformsVec (valid_trans);
micvfh_estimator_->getTransformsVec (transforms);
std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
bf::path desc_dir = path;
if (!bf::exists (desc_dir))
bf::create_directory (desc_dir);
std::stringstream path_view;
path_view << path << "/view_" << v << ".pcd";
pcl::io::savePCDFileBinary (path_view.str (), *processed);
std::stringstream path_pose;
path_pose << path << "/pose_" << v << ".txt";
PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v));
std::stringstream path_entropy;
path_entropy << path << "/entropy_" << v << ".txt";
PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v));
//save signatures and centroids to disk
for (size_t j = 0; j < signatures.size (); j++)
{
if (valid_trans[j])
{
std::stringstream path_centroid;
path_centroid << path << "/centroid_" << v << "_" << j << ".txt";
Eigen::Vector3d centroid (centroids[j][0], centroids[j][1], centroids[j][2]);
PersistenceUtils::writeCentroidToFile (path_centroid.str (), centroid);
std::stringstream path_descriptor;
path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd";
pcl::io::savePCDFileBinary (path_descriptor.str (), signatures[j]);
//save roll transform
std::stringstream path_pose;
path_pose << path << "/roll_trans_" << v << "_" << j << ".txt";
PersistenceUtils::writeMatrixToFile (path_pose.str (), transforms[j]);
}
}
}
}
else
{
//else skip model
std::cout << "The model has already been trained..." << std::endl;
//there is no need to keep the views in memory once the model has been trained
models->at (i).views_->clear ();
}
}
//load features from disk
//initialize FLANN structure
loadFeaturesAndCreateFLANN ();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "model_fitting");
ros::NodeHandle n;
ros::Rate loop_rate(0.8);
boost::normal_distribution<> nd(0.0, 0.01);
boost::mt19937 rng;
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> >
var_nor(rng, nd);
ros::Publisher model_pub = n.advertise<articulation_model_msgs::ModelMsg> ("model_track", 5);
// ------------------- generate data ----------------------------------
bool rotational = true;
bool prismatic = false;
bool rigid = false;
const int datapoints_number = 300;
std::vector <geometry_msgs::Pose> generated_poses;
for (int i = 0; i < datapoints_number; i++)
{
geometry_msgs::Pose pose;
if (prismatic)
{
pose.position.x = 2 + (static_cast<float> (i)/100.0) + var_nor();
pose.position.y = (static_cast<float> (i)/100.0) + var_nor();
pose.position.z = (static_cast<float> (i)/100.0) + var_nor();
}
else if (rotational)
{
pose.position.x = 2 + cos(static_cast<float> (i) / 100.0) + var_nor();
pose.position.y = sin(static_cast<float> (i) / 100.0) + var_nor();
pose.position.z = var_nor();
}
else if(rigid)
{
pose.position.x = 2 + var_nor();
pose.position.y = 4 + var_nor();
pose.position.z = 1 + var_nor();
}
if (rotational)
{
double yaw = static_cast<float> (i)/100;
double roll = M_PI/4;
double pitch = 0;
tf::Quaternion tf_pose_quat;
tf_pose_quat.setRPY(roll, pitch, yaw);
pose.orientation.w = tf_pose_quat.getW();
pose.orientation.x = tf_pose_quat.getX();
pose.orientation.y = tf_pose_quat.getY();
pose.orientation.z = tf_pose_quat.getZ();
}
else
{
pose.orientation.x = 0;
pose.orientation.y = 0;
pose.orientation.z = 0;
pose.orientation.w = 1;
}
generated_poses.push_back(pose);
}
const int initial_datapoints_number = 40;
articulation_model_msgs::ModelMsg model_msg;
articulation_model_msgs::ParamMsg sigma_param;
sigma_param.name = "sigma_position";
sigma_param.value = 0.02;
sigma_param.type = articulation_model_msgs::ParamMsg::PRIOR;
model_msg.params.push_back(sigma_param);
model_msg.track = generateMeasurement(generated_poses, 0, initial_datapoints_number);
std::cout << "creating object" << std::endl;
ArticulationModelPtr rotational_instance(new RotationalModel);
ArticulationModelPtr prismatic_instance(new PrismaticModel);
ArticulationModelPtr rigid_instance(new RigidModel);
model_msg.name = "rotational";
rotational_instance->setModel(model_msg);
model_msg.name = "prismatic";
prismatic_instance->setModel(model_msg);
model_msg.name = "rigid";
rigid_instance->setModel(model_msg);
std::cout << "fitting" << std::endl;
// rotational_instance->fitModel();
// prismatic_instance->fitModel();
// rigid_instance->fitModel();
int loop_count = 1;
int generate_measurement_every_so_many = 10;
while (ros::ok())
{
if (loop_count % generate_measurement_every_so_many == 0)
{
// ------------------------ generate measurement -------------------------------
articulation_model_msgs::TrackMsg z;
z = generateMeasurement(generated_poses, loop_count + initial_datapoints_number - generate_measurement_every_so_many, loop_count + initial_datapoints_number + 10);
rotational_instance->addTrack(z);
prismatic_instance->addTrack(z);
rigid_instance->addTrack(z);
std::cout << "measurement taken" << std::endl;
}
std::cout << "evaluating" << std::endl;
rotational_instance->fitModel();
prismatic_instance->fitModel();
rigid_instance->fitModel();
rotational_instance->evaluateModel();
prismatic_instance->evaluateModel();
rigid_instance->evaluateModel();
std::cout << "done" << std::endl;
// std::cout << "model class = "<< model_instance->getModelName() << std::endl;
// std::cout << " radius = "<<rotational_instance->getParam("rot_radius")<< std::endl;
// boost::shared_ptr<RotationalModel> rotational = boost::dynamic_pointer_cast< RotationalModel > (model_instance);
// std::cout << " rotational radius = "<<rotational->rot_radius<< std::endl;
// tf::Matrix3x3 m(rotational->rot_axis);
// double yaw, pitch, roll;
// m.getEulerYPR(yaw, pitch, roll);
// std::cout << " rotational yaw = "<<yaw<< std::endl;
// std::cout << " rotational pitch = "<<pitch<< std::endl;
// std::cout << " rotational roll = "<<roll<< std::endl;
//// std::cout << " rigid_position.x = "<<model_instance->getParam("rigid_position.x")<< std::endl;
//// boost::shared_ptr<RigidModel> rigid = boost::dynamic_pointer_cast< RigidModel > (model_instance);
//// std::cout << " pos.x = " << rigid->pos_x << std::endl;
double rotational_loglikelihood = rotational_instance->getParam("loglikelihood");
double prismatic_loglikelihood = prismatic_instance->getParam("loglikelihood");
double rigid_loglikelihood = rigid_instance->getParam("loglikelihood");
std::cout << " rotational log LH = " << rotational_loglikelihood<< "\n " << std::endl;
std::cout << " prismatic log LH = " << prismatic_loglikelihood<< "\n " << std::endl;
std::cout << " rigid log LH = " << rigid_loglikelihood << "\n \n" << std::endl;
double max_loglikehood = std::max(std::max(rotational_loglikelihood, prismatic_loglikelihood), rigid_loglikelihood);
rotational_loglikelihood = rotational_loglikelihood - max_loglikehood;
prismatic_loglikelihood = prismatic_loglikelihood - max_loglikehood;
rigid_loglikelihood = rigid_loglikelihood - max_loglikehood;
double rotational_likelihood = exp(rotational_loglikelihood);
double prismatic_likelihood = exp(prismatic_loglikelihood);
double rigid_likelihood = exp(rigid_loglikelihood);
double sum_likelihoods = rotational_likelihood + prismatic_likelihood + rigid_likelihood;
rotational_likelihood = rotational_likelihood/sum_likelihoods;
prismatic_likelihood = prismatic_likelihood/sum_likelihoods;
rigid_likelihood = rigid_likelihood/sum_likelihoods;
std::cout << " rotational normalized likelihood = " << rotational_likelihood<< "\n " << std::endl;
std::cout << " prismatic normalized likelihood = " << prismatic_likelihood<< "\n " << std::endl;
std::cout << " rigid normalized likelihood = " << rigid_likelihood << "\n \n" << std::endl;
rotational_instance->setParam("weight",rotational_likelihood,articulation_model_msgs::ParamMsg::EVAL);
prismatic_instance->setParam("weight",prismatic_likelihood,articulation_model_msgs::ParamMsg::EVAL);
rigid_instance->setParam("weight",rigid_likelihood,articulation_model_msgs::ParamMsg::EVAL);
std::cout << " rotational BIC = " << rotational_instance->getParam("bic")<< "\n " << std::endl;
std::cout << " prismatic BIC = " << prismatic_instance->getParam("bic")<< "\n " << std::endl;
std::cout << " rigid BIC = " << rigid_instance->getParam("bic") << "\n \n" << std::endl;
// std::cout << " points= "<<model_msg.track.pose.size() << "\n " << std::endl;
model_pub.publish(rotational_instance->getModel());
model_pub.publish(prismatic_instance->getModel());
model_pub.publish(rigid_instance->getModel());
ros::spinOnce();
loop_rate.sleep();
loop_count++;
}
}
