/// \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; }