// RK4 integration void double_integrator_dynamics( double *z, double *f, void *user_data ) { // Double integrator dynamics, 2d int num_states = 4; int num_controls = 2; MatrixXd A(num_states, num_states); MatrixXd B(num_states, num_controls); Vector4d x_t(z[0], z[1], z[2], z[3]); Vector2d u_t(z[4], z[5]); double t = z[6]; //double t = ((double*) user_data)[0]; A.topRightCorner(num_states/2, num_states/2).setIdentity(); B.bottomRows(num_controls).setIdentity(); Vector4d k1 = A * x_t + B * u_t; Vector4d k2 = A * (x_t + (k1.array()/2.0).matrix()) + B * u_t; Vector4d k3 = A * (x_t + (k2.array()/2.0).matrix()) + B * u_t; Vector4d k4 = A * (x_t + k3) + B * u_t; Vector4d x_new = (x_t.array() + t/6 * (k1.array() + 2*k2.array() + 2*k3.array() + k4.array())).matrix(); f[0] = x_new(0); f[1] = x_new(1); f[2] = x_new(2); f[3] = x_new(3); }
double rhs_2nd_equation (double x, double t, Gas_parameters * parameters) { return u_t (x, t) + u_exact (x, t) * u_x (x, t) + parameters->p_ro * g_x (x, t) - parameters->viscosity * exp (-g_exact (x, t)) * u_xx (x, t); }
int main(int argc, char** argv) { init(argc, argv); std::string image_format = argv[1]; int n = boost::lexical_cast<int>(argv[2]); CHECK(n > 0) << "Need at least one frame, two would be great"; cv::Size screen_size(FLAGS_width, FLAGS_height); double rho = FLAGS_rho; double lambda = FLAGS_lambda; // Pre-load images. LOG(INFO) << "Loading images..."; std::vector<cv::Mat> color_images; loadImages(image_format, n, screen_size, color_images); // Convert to grayscale. LOG(INFO) << "Loaded " << color_images.size() << " images"; std::vector<cv::Mat> gray_images; std::vector<cv::Mat>::const_iterator color_image; for (color_image = color_images.begin(); color_image != color_images.end(); ++color_image) { cv::Mat gray_image; cv::cvtColor(*color_image, gray_image, CV_BGR2GRAY); gray_images.push_back(gray_image); } TrackList<cv::Point2d> track; bool exit = false; State state; state.clicked = false; cv::namedWindow("video"); cv::setMouseCallback("video", onMouse, &state); // Display the image and ask the user to click a point. cv::imshow("video", color_images[0]); while (!state.clicked) { cv::waitKey(10); } // Extract a patch for cross-correlation. cv::Mat original; { cv::Point offset(state.point.x - RADIUS, state.point.y - RADIUS); cv::Rect region(offset, cv::Size(DIAMETER, DIAMETER)); original = gray_images.front()(region); } // Match the template to every image. std::vector<cv::Mat> responses; LOG(INFO) << "Performing cross-correlation..."; std::vector<cv::Mat>::const_iterator image; for (image = gray_images.begin(); image != gray_images.end(); ++image) { // Evaluate response to template. cv::Mat response; cv::matchTemplate(*image, original, response, cv::TM_CCORR_NORMED); // Convert to 64-bit. response = cv::Mat_<double>(response); response = -1. * response; // Add to list. responses.push_back(response); } // Solve dynamic program. cv::Mat x_star = cv::Mat_<double>(n, 2, 0.); LOG(INFO) << "Solving dynamic program..."; { std::vector<cv::Mat> scaled; std::vector<cv::Mat>::const_iterator response; for (response = responses.begin(); response != responses.end(); ++response) { scaled.push_back(cv::Mat(1. / lambda * (*response))); } std::vector<cv::Vec2i> x; solveViterbiQuadratic2D(scaled, x); std::vector<cv::Vec2i>::const_iterator x_t; int t = 0; for (x_t = x.begin(); x_t != x.end(); ++x_t) { x_star.at<double>(t, 0) = (*x_t)[1]; x_star.at<double>(t, 1) = (*x_t)[0]; t += 1; } } LOG(INFO) << "Solving ADMM..."; // Guess Lagrange multipliers to all be zero. cv::Mat x = cv::Mat_<double>(n, 2, 0.); // Initialize track positions to origin. cv::Mat z = cv::Mat_<double>(n, 2, 0.); // Smoothest path will also be origin. cv::Mat u = cv::Mat_<double>(n, 2, 0.); while (!exit) { LOG(INFO) << "rho => " << rho; // Solve first sub-problem. for (int t = 0; t < n; t += 1) { cv::Point2d z_t(z.at<double>(t, 0), z.at<double>(t, 1)); cv::Point2d u_t(u.at<double>(t, 0), u.at<double>(t, 1)); cv::Point2d x_t; findBestResponse(z_t, u_t, responses[t], rho, x_t); x.at<double>(t, 0) = x_t.x; x.at<double>(t, 1) = x_t.y; } //LOG(INFO) << "x-update: norm(x - z) => " << cv::norm(x - z) / n; cv::Mat z_old = z.clone(); // Solve second sub-problem. for (int d = 0; d < 2; d += 1) { cv::Mat dst = z.col(d); findSmoothestPath(x.col(d), u.col(d), rho, lambda, dst); } //LOG(INFO) << "z-update: norm(x - z) => " << cv::norm(x - z) / n; // Update multipliers. u += x - z; cv::Mat r = x - z; cv::Mat s = rho * (z - z_old); LOG(INFO) << "norm(r) => " << cv::norm(r); LOG(INFO) << "norm(s) => " << cv::norm(s); LOG(INFO) << "norm(x - x_star) / n => " << cv::norm(x - x_star) / n; // Play the video. for (int t = 0; t < n; t += 1) { cv::Mat display = color_images[t].clone(); // Draw tracks on frame. cv::Point2d pt1; cv::Point2d pt2; pt1 = cv::Point2d(x_star.at<double>(t, 0), x_star.at<double>(t, 1)); pt2 = pt1 + cv::Point2d(DIAMETER, DIAMETER); cv::rectangle(display, pt1, pt2, cv::Scalar(0, 0x99, 0), 2); pt1 = cv::Point2d(z.at<double>(t, 0), z.at<double>(t, 1)); pt2 = pt1 + cv::Point2d(DIAMETER, DIAMETER); cv::rectangle(display, pt1, pt2, cv::Scalar(0xFF, 0, 0), 2); pt1 = cv::Point2d(x.at<double>(t, 0), x.at<double>(t, 1)); pt2 = pt1 + cv::Point2d(DIAMETER, DIAMETER); cv::rectangle(display, pt1, pt2, cv::Scalar(0, 0, 0xFF), 2); cv::imshow("video", display); cv::waitKey(10); } if (rho < 1.) { cv::Mat y = rho * u; rho = rho * FLAGS_rho_step; u = 1. / rho * y; } } return 0; }