/// \brief Unscented transform of process Sigma points
void UnscentedKalmanFilter::utf(vnl_matrix<double> X, vnl_vector<double> u,
vnl_vector<double> &y, vnl_matrix<double> &Y, vnl_matrix<double> &P, vnl_matrix<double> &Y1)
{
// determine number of sigma points
unsigned int L = X.cols();
// zero output matrices
y.fill(0.0); Y.fill(0.0);
// transform the sigma points and put them as columns in a matrix Y
for( int k = 0; k < L; k++ )
{
vnl_vector<double> xk = X.get_column(k);
vnl_vector<double> yk(N);
f(xk,u,yk);
Y.set_column(k,yk);
// add each transformed point to the weighted mean
y = y + Wm.get(0,k)*yk;
}
// create a matrix with each column being the weighted mean
vnl_matrix<double> Ymean(N,L);
for( int k = 0; k < L; k++ )
Ymean.set_column(k,y);
// set the matrix of difference vectors
Y1 = Y-Ymean;
// calculate the covariance matrix output
vnl_matrix<double> WC(L,L,0.0);
WC.set_diagonal(Wc.get_row(0));
P = Y1*WC*Y1.transpose();
}
/// \brief Nonlinear process model for needle steering
void UnscentedKalmanFilter::f(vnl_vector<double> x1, vnl_vector<double> u,
vnl_vector<double> &x2)
{
// initialize the output to 0
x2.fill(0.0);
// isolate the position and orientation components of the input vector
vnl_vector<double> p = x1.extract(3,0);
vnl_vector<double> r = x1.extract(3,3);
// change rotation vector representation to quaternion
vnl_vector<double> r_norm = r;
r_norm.normalize();
vnl_quaternion<double> q(r_norm,r.magnitude());
// isolate specific input variables
double d_th = u[0];
double ro = u[1];
double l = u[2];
// define x,z axes as vectors
vnl_matrix<double> I(3,3); I.set_identity();
vnl_vector<double> x_axis = I.get_column(0);
vnl_vector<double> z_axis = I.get_column(2);
// Update position
// define rotation matrix for d_th about z_axis
vnl_matrix<double> Rz_dth = vnl_rotation_matrix( (d_th*z_axis) );
// define circular trajectory in y-z plane
vnl_vector<double> circ(3,0.0);
circ[1] = ro*(1-cos(l/ro));
circ[2] = ro*sin(l/ro);
// define delta position vector in current frame
vnl_vector<double> d_p = Rz_dth*circ;
// Transform delta vector into world frame using quaternion rotation
vnl_vector<double> d_p_world = q.rotate(d_p);
// add rotated vector to original position
vnl_vector<double> p2 = d_p_world + p;
// Update orientation
// form quaternions for x-axis and z-axis rotations
vnl_quaternion<double> q_b(z_axis,d_th);
vnl_quaternion<double> q_a(x_axis,-l/ro);
// multiply original orientation quaternion
vnl_quaternion<double> q2 = q*q_b*q_a;
vnl_vector<double> r2 = q2.axis()*q2.angle();
// Compose final output
for( int i = 0; i < 3; i++)
{
x2[i] = p2[i];
x2[i+3] = r2[i];
}
}
bool arlCore::fieldCalibration( PointList::csptr real, PointList::csptr distorded, unsigned int degree, vnl_vector<double> ¶meters, double &RMS )
{
if(degree<1) return false;
Polynomial_cost_function pcf( real, distorded, degree );
vnl_powell op(&pcf);
parameters.set_size(pcf.getNbParameters());
parameters.fill(0.0);
op.minimize(parameters);
RMS = op.get_end_error();
return true;
}
void TpsRegistration::SetParam(vnl_vector<double>& x0) {
int k = 0;
x0.set_size(func_->get_number_of_unknowns());
x0.fill(0);
if (!func_->fix_affine_) { // x0 includes affine
for (int i = 0; i < param_affine_.rows(); ++i) {
for (int j = 0; j < d_; ++j, ++k) {
x0[k] = param_affine_(i, j);
}
}
}
for (int i = 0; i < param_tps_.rows(); ++i) {
for (int j = 0; j < d_; ++j, ++k) {
x0[k] = param_tps_(i, j);
}
}
}
void histogram(const vector<CvPoint> &tplt, IplImage *img, vnl_vector<double> &hist, unsigned iPerBin = 1)
{
vector<scanline_t> mask;
getMask(tplt,mask);
if (hist.size() == 0) {
hist.set_size(256/iPerBin);
hist.fill(0.0);
}
unsigned char *src = (unsigned char *)img->imageData;
unsigned K = img->nChannels, denom = K*iPerBin;
for (unsigned i=0; i<mask.size(); ++i) {
unsigned char *s=src+mask[i].line*img->widthStep + K*mask[i].start;
for (unsigned j=mask[i].start; j<mask[i].end; ++j) {
unsigned val=0.;
for (unsigned k=0; k!=K; ++k, ++s) {
val += *s;
*s = 0;
}
val /= denom;
hist[val]++;
}
}
}
int main(int argc, char **argv)
{
string params_file;
// handling arguments
po::options_description optionsDescription(
"Human Detection main function\n"
"Available remappings:\n"
" humanDetections:=<humanDetections-topic>\n"
"\n"
"Allowed options");
optionsDescription.add_options()
("help,h","show help message")
("params_file,p", po::value<string>(¶ms_file)->required(),"filename of params.xml")
("num_persons,n", po::value<unsigned int>(&NUM_PERSONS)->default_value(4))
("visualize,v","visualize detection\n")
("threads,t", po::value<unsigned int>(&nrThreads)->default_value(4),"number of threads used");
("save_all,a", po::value<string>(&save_all)->default_value(""),"save all data\n");
po::variables_map variablesMap;
try
{
po::store(po::parse_command_line(argc, argv, optionsDescription),variablesMap);
if (variablesMap.count("help")) {cout<<optionsDescription<<endl; return 0;}
po::notify(variablesMap);
}
catch (const std::exception& e)
{
std::cout << "--------------------" << std::endl;
std::cerr << "- " << e.what() << std::endl;
std::cout << "--------------------" << std::endl;
std::cout << optionsDescription << std::endl;
return 1;
}
visualize=variablesMap.count("visualize"); // are we visualizing the frames and detections?
if (!save_all.empty())
{
sprintf(data_file,"%s/data.xml",save_all.c_str());
}
string path, bgmodel_file, prior_file, frame_file;
boost::filesystem::path p(params_file);
path = p.parent_path().string().c_str();
bgmodel_file = path + "/" + "bgmodel.xml";
prior_file = path + "/" + "prior.txt";
frame_file = path + "/" + "frame.dat";
ROS_INFO_STREAM("loading '"<<bgmodel_file.c_str()<<"'");
// load coordinate frame of camera
if (!load_msg(frame,frame_file))
{
std::cerr<<"Failed to load the coordinate frame from file '"<<frame_file<<"'. Use program 'create_frame' to create the file."<<endl;
exit(1);
}
ROS_INFO_STREAM("loading '"<<bgmodel_file.c_str()<<"'");
// Initialize localization module
#if USE_DYNAMIC_BACKGROUND
#else
getBackground(bgmodel_file.c_str(), bgModel); // load background model
#endif
ROS_INFO_STREAM("loading '"<<params_file.c_str()<<"'");
loadCalibrations(params_file.c_str()); // load calibration
ROS_INFO_STREAM("loading '"<<prior_file.c_str()<<"'");
loadHull(prior_file.c_str(), priorHull);
#if USE_DYNAMIC_BACKGROUND
#else
assert_eq(bgModel.size(), CAM_NUM);
assert_eq(cam.size(), bgModel.size());
#endif
ROS_INFO_STREAM("generate Scan locations");
genScanLocations(priorHull, scanres, scanLocations);
logLocPrior.set_size(scanLocations.size());
logLocPrior.fill(-log(scanLocations.size()));
ROS_INFO_STREAM("loading done\n");
// ROS nodes, subscribers and publishers
ros::init(argc, argv, "camera_localization");
ros::NodeHandle n;
std::string resolved_humanDetections=n.resolveName("humanDetections");
cout<<"publish to detection topic: "<<resolved_humanDetections<<endl;
tf::TransformBroadcaster transformBroadcaster;
transformBroadcasterPtr=&transformBroadcaster;
image_transport::ImageTransport it(n);
humanDetectionsPub = n.advertise<accompany_uva_msg::HumanDetections>(resolved_humanDetections, 10);
markerArrayPub = n.advertise<visualization_msgs::MarkerArray>("visualization_marker_array",0);
frameCounter=new FrameCounter(cam.size());
backgroundsPub=new image_transport::Publisher[cam.size()];
detectionsPub=new image_transport::Publisher[cam.size()];
for (unsigned int i=0;i<cam.size();i++)
{
// subscribe to images
string topicName=cam[i].name+"/image_raw";
ROS_INFO_STREAM("subscribe to '"<<topicName<<"'");
image_transport::CameraSubscriber sub = it.subscribeCamera(topicName,1,imageCallback);
// init publishers for background images
string backgroundTopicName=cam[i].name+"/background/image_raw";
ROS_INFO_STREAM("publish to '"<<backgroundTopicName<<"'");
backgroundsPub[i]=it.advertise(backgroundTopicName,1);
// init publishers for detection images
string detectTopicName=cam[i].name+"/detect/image_raw";
ROS_INFO_STREAM("publish to '"<<detectTopicName<<"'");
detectionsPub[i]=it.advertise(detectTopicName,1);
// init images
src_vec[i]=NULL;
}
boost::thread publishTFThread(publishTF);
//ros::Timer timer=n.createTimer(ros::Duration(0.01),timerCallback); // use thread to avoid waiting on processing frames
ROS_INFO_STREAM("wait for frames");
ros::spin();
running=false;
publishTFThread.join();
for (unsigned int c=0;c<cam.size();c++)
if (src_vec[c]!=NULL)
cvReleaseImage(&src_vec[c]);
return 0;
}
