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