text:
/*
  LODO library: laser-stabilized odometry
  Copyright (C) 2004 Andrew Howard  ahoward@usc.edu

  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU General Public License
  as published by the Free Software Foundation; either version 2
  of the License, or (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
*/

/** @file lodo.h

@brief The lodo library provides laser-stabilizied odometric pose
estimates: the inputs are raw odometry and laser scans, the output is
a corrected pose estimate.  The drift in this corrected estimate is
much lower than that seen with odometry alone.  See, for example, the
figure below: this plot shows the trajectory of a robot that has
travelled 125m and executed 19 complete rotations before returning to
the starting location.  The final cumulative orientation error is less
than 5 degrees (versus 110 degrees for pure odometry).

@image html lodo_comparison.gif "Odometry versus laser-stabilized odometry"

@par How it works

The lodo library uses an incremental SLAM algorithm to correct drift
in the robot's orientation.  The algorithm has three key data structures:

- The current laser scan (a list of range values).
- The local map (a ring buffer containing recent laser scans).
- The polar error function (a grid generated from the local map).

The algorithm applied to each new laser scan is as follows:

-# Generate the polar error function:
  - For each scan in the local map:
    - For each point on the polygonal boundary of the map scan:
      - Project point into the local coordinate frame of the new scan.
      - Convert to polar coordinates and mark the corresponding cell
        in the polar error function as "occupied".
  - For each cell in the polar error function, compute the distance
    to the nearest occupied cell with the same radial value.
-# Find orientation correction:
  - For each possible correction:
    - Apply correction to each point in the new scan and convert into
      polar coordinates.
    - Look up error value in the polar error function.
    - Add to the todal error for this correction value.
  - Select correction value with the lowest total error.
-# Build map:
  - If the new scan has a signficant number of "outlier" points
    (large error values), add this scan to the current map.  The map
    is a ring buffer, so adding a new scan will necessarily lead to
    an old one being discarded.

The intuition behind this algorithm is best explained in diagrams (below).

Figure (a) shows a map scan in global carestian coordinates; the map
scan is projected into the coordinate frame of the new scan and
converted to polar coordinates (b); using dynamic programming, we
compute the error function (c).  This polar error function can be used
to test possible orientation corrections very efficiently: changes in
the orientation of the new scan correspond the translations of the
polar error function.

 @image html lodo_map.gif "(a) Original map scan (cartesian coordinates)."
 @image html lodo_map_polar.gif "(b) Map scan in local polar coordinates."

 @image html lodo_map_pef.gif "(c) Calculated polar error function."

@par lodo_caveats

The current version of the library takes about 15ms to process each
scan on a 2.8 GHz P4, so expect it to use lots of cycles on your
robot.  There is still lots of optimization to do, however, so expect
future releases to clock in around the 5ms mark.

@todo Accuracy, reliability and performance

*/

#ifndef LODO_H
#define LODO_H

#include <gsl/gsl_min.h>
#include "slap.h"

#ifdef __cplusplus
extern "C"
{
#endif

/// Limits
#define LODO_MAX_RANGES 1024

/// @brief Data pertaining to an individual scan
typedef struct
{
  /// Odometric pose (x, y, theta)
  pose2_t opose;

  /// Corrected pose (x, y, theta)
  pose2_t cpose;

  /// Range values
  double ranges[LODO_MAX_RANGES];

} lodo_scan_t;

/// @brief Working data for a map point
typedef struct
{
  /// Local cartesian position
  vector3_t local;

  /// Local polar position
  vector3_t polar;

} lodo_map_point_t;

/// @brief Working data for scan point
typedef struct
{
  /// Range bin in PEF
  int ni;

} lodo_scan_point_t;

/// @brief Laser odometry module data
typedef struct
{
  /// Number of points in each scan
  int num_ranges;

  /// Maximum accepted range
  double range_max;

  /// Start angle for scans
  double range_start;

  /// Angular resolution of scans
  double range_step;

  /// Laser pose relative to robot
  pose2_t laser_pose;

  /// Maximum error value on a single point (m)
  double max_error;

  /// Number of bins in polar error function
  int pef_num_ranges, pef_num_bearings;

  /// Range bin starting value and resolution in polar error function.
  double pef_range_start, pef_range_step;

  /// Bearing bin starting value and resolution in polar error
  /// function.  Note that the pef_bearing_step measures a distance,
  /// not an angle.
  double pef_bearing_start, pef_bearing_step;

  /// Polar error function (map)
  int pef_size;
  int *pef;

  /// Search interval for fitting (fitting will check interval
  /// [-fit_interval, +fit_interval] radians).
  double fit_interval;

  /// Error threshold for good fits (error must be less than this
  /// value for a good fit).
  double fit_err_thresh;

  /// Error value for outlier points (points with error values larger
  /// than this are labeled as outliers).
  double fit_outlier_dist;

  /// Outlier fraction threshold (used to decide when scans should be
  /// added to the map).
  double fit_outlier_frac;

  /// Cumulative odometric distance and rotation
  double odom_dist, odom_turn;

  /// Odometer interval for adding scans.
  double map_dist_interval, map_turn_interval;

  /// Odometer value of last scan in map
  double map_last_dist, map_last_turn;

  /// Map (ring buffer of scans)
  int map_scan_count, max_map_scans;
  lodo_scan_t *map_scans;

  /// Current scan
  int scan_count;
  lodo_scan_t scan;

  /// Working space for current scan (useful pre-computed values).
  lodo_scan_point_t scan_points[LODO_MAX_RANGES];

  /// Working space for projected map points
  int num_map_points, max_map_points;
  lodo_map_point_t *map_points;

  /// Minimizer
  gsl_min_fminimizer *mini;

  /// Fit correction
  double fit_correct;

  /// True if last scan fitted
  int fit_valid;

  /// True if the last scan should be added to the map
  int fit_add;

} lodo_t;

/// @brief Allocate object
/// @param num_ranges Number of range readings in each scan (should be 181).
/// @param range_max Maximum useable range value (e.g., 8.00 or 16.00).
/// @param range_res Resolution for comparing range values (i.e., range bin width).
/// @param range_start Starting angle for range readings (should be -M_PI / 2).
/// @param range_step Angular step size for each successive range reading
///                   (should be M_PI / 180).
/// @returns Object handle.
lodo_t *lodo_alloc(int num_ranges, double range_max, double range_res,
                   double range_start, double range_step);

/// @brief Free object
void lodo_free(lodo_t *self);

/// @brief Add a scan, compute correction, update map
/// @param self Object handle.
/// @param odom_pose Raw odometric pose of the robot.
/// @param num_ranges Number of range readings.
/// @param ranges Array of laser range readings.
/// @returns Returns the corrected robot pose.
pose2_t lodo_add_scan(lodo_t *self, pose2_t odom_pose, int num_ranges, double *ranges);

/// @brief Project map points into scan polar coordinates
/// @internal
void lodo_project_map(lodo_t *self);

/// @brief Project map free space into the scan polar frame
/// @internal
void lodo_project_map_free(lodo_t *self, lodo_scan_t *scan_m,
                           matrix33_t Pd, matrix33_t Pm);

/// @brief Compute correction for a scan
/// @internal
double lodo_correct(lodo_t *self);

/// @brief Test a scan offset
/// @internal
double lodo_test_offset(lodo_t *self, double offset, double *outliers);

/// @brief Print projected map points
void lodo_print_map(lodo_t *self);

/// @brief Print a polar error functions for scan and map
void lodo_print_pef(lodo_t *self);

/// @brief Print error histograms
void lodo_print_err(lodo_t *self);

/// @brief Draw a scan
void lodo_draw_scan(lodo_t *self, lodo_scan_t *scan);

/// @brief Draw the current map (hits)
void lodo_draw_map(lodo_t *self);

#ifdef __cplusplus
}
#endif

#endif


text:
/* ode-initval/rk4.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Runge-Kutta 4, Classical */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

typedef struct
{
  double *k;
  double *y0;
  double *ytmp;
}
rk4_state_t;

static void *
rk4_alloc (size_t dim)
{
  rk4_state_t *state = (rk4_state_t *) malloc (sizeof (rk4_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rk4_state", GSL_ENOMEM);
    }

  state->k = (double *) malloc (dim * sizeof (double));

  if (state->k == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k", GSL_ENOMEM);
    }

  state->y0 = (double *) malloc (dim * sizeof (double));

  if (state->y0 == 0)
    {
      free (state->k);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->y0);
      free (state->k);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  return state;
}

static int
rk4_apply (void *vstate,
           size_t dim,
           double t,
           double h,
           double y[],
           double yerr[],
           const double dydt_in[],
           double dydt_out[],
           const gsl_odeiv_system * sys)
{
  rk4_state_t *state = (rk4_state_t *) vstate;

  size_t i;
  int status = 0;

  double *const k = state->k;
  double *const y0 = state->y0;
  double *const ytmp = state->ytmp;

  /* Copy the starting value. We will write over
   * the y[] vector, using it for scratch and
   * then filling it with the final result.
   */

  DBL_MEMCPY (y0, y, dim);

  if (dydt_in != NULL)
    {
      DBL_MEMCPY (k, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y0, k);
      GSL_STATUS_UPDATE (&status, s);
    }

  for (i = 0; i < dim; i++)
    {
      y[i] = h / 6.0 * k[i];    /* use y[] to store delta_y */
      ytmp[i] = y0[i] + 0.5 * h * k[i];
    }

  /* k2 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);
    GSL_STATUS_UPDATE (&status, s);
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 3.0 * k[i];
      ytmp[i] = y0[i] + 0.5 * h * k[i];
    }

  /* k3 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);
    GSL_STATUS_UPDATE (&status, s);
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 3.0 * k[i];
      ytmp[i] = y0[i] + h * k[i];
    }

  /* k4 step, error estimate, and final sum */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k);
    GSL_STATUS_UPDATE (&status, s);
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 6.0 * k[i];
      yerr[i] = h * y[i];
      y[i] += y0[i];
      if (dydt_out != NULL)
        dydt_out[i] = k[i];
    }

  return status;
}

static int
rk4_reset (void *vstate, size_t dim)
{
  rk4_state_t *state = (rk4_state_t *) vstate;

  DBL_ZERO_MEMSET (state->k, dim);
  DBL_ZERO_MEMSET (state->y0, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk4_order (void *vstate)
{
  rk4_state_t *state = (rk4_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 4;
}

static void
rk4_free (void *vstate)
{
  rk4_state_t *state = (rk4_state_t *) vstate;
  free (state->k);
  free (state->y0);
  free (state->ytmp);
  free (state);
}

static const gsl_odeiv_step_type rk4_type = { "rk4",    /* name */
  1,                            /* can use dydt_in */
  0,                            /* gives exact dydt_out */
  &rk4_alloc,
  &rk4_apply,
  &rk4_reset,
  &rk4_order,
  &rk4_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk4 = &rk4_type;

ext: c


text:
/////////////////
//example25.5.c
/////////////////
#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#include <math.h>

typedef struct{
  double *a; //y=a[0]x[0]+a[1]x[1]+...a[ka]y[ka]+b[0]u[0]+b[1]u[1]+...+b[kb]u[kb]
  //  double *a; //y[t+1]=a[0]y[t]+a[1]y[t-1]+...a[ka]y[t-ka]+b[0]u[t]+b[1]u[t-1]+...+b[kb]u[t-kb]
  double *b;
  double *x;
  double *u;
  int k,kx,ku;
  double xmax;
  double umax;
  double y;
  int _dim;
  double *_y;
  double *_y_err;
  double *_dydt_in;
  double *_dydt_out;
  double _t;
} CRANE;

#define dim_crane 4
int cranefunc (double t, const double y[], double f[], void *params)
{
  CRANE *c = (CRANE *)params;
  f[0] = y[1];
  f[1] = (-(2*c->dr+c->C)*y[1] -c->ddX*cos(y[0]) -c->g*sin(y[0]))/c->r;
  f[2] = y[3];
  f[3] = (c->F+c->T*sin(y[0]))/c->M;
  return GSL_SUCCESS;
}
////////////////
#define square(x) ((x)*(x))
CRANE crane;
int initialize()
{
  crane.kx=3;
  crane.ku=1;
  crane.k=crane.kx+ku;
  double *a=(double*)malloc(sizeof(double)*ka);
  double *b=&a[kx];
  double *x=(double*)malloc(sizeof(double)*k+1);
  double *u=&x[kx];
  crane.umax=20.0;//check//  crane.Fmax=30;//check

#ifdef CRANESUB
  //  crane.Fmax=_crane_Fmax;//check//  crane.Fmax=30;//check
  //  crane.xmax=20.0;//check//  crane.Fmax=30;//check
  if(_AP_umax>0) AP_u_max=crane.umax=_AP_umax;
  else AP_u_max=crane.umax;

  AP_u_min=-AP_u_max;
  rr=AP_r=_AP_r;//10
  rr_kyoyou=_rr_kyoyou;
  C_MODE=11;
#else
  crane.h=0.01;
#endif
  crane._t=0;
  int i;for(i=0;i<crane._dim;i++) crane._dydt_in[i]=0;

  return(0);
}
#ifndef CRANESUB
char *fn="crane2io.dat";
FILE *fp;
#endif//#ifndef CRANESUB

double plant(double uu)
{
  //store previous x and u
  int i,kx_1=crane.kx-1;
  for(i=0;i<kx_1;i++) crane.x[i+1]=crane.x[i];
  int ku_1=crane.ku-1;
  for(i=0;i<ku_1;i++) crane.u[i+1]=crane.u[i];
  //store former output
  crane.x[0]=crane.y;

  //current input
  if(uu>crane.umax) crane.u[0]=crane.umax;
  else if(uu<-crane.umax) crane.u[0]=-crane.umax;
  // current output
  crane.y=0;
  for(i=0;i<crane.k;i++) crane.y+=crane.a[i]*crane.x[i];
  crane._t++;

#ifndef CRANESUB
  fprintf(fp,"%.7e %.7e %.7e %.7e", crane._t,crane.u[0],crane.x[0],crane.y);//crane.a,crane.da
  fprintf(fp,"\n");
#endif
  return(crane.x);
  //  return(crane.X);
}

#ifndef CRANESUB
int main()
{
  ////////////////////////////////////////////////////
  /// method 1                                     ///
  /// input ddX                                    ///
  /// output a,x,y,F                               ///
  ////////////////////////////////////////////////////
  //  double h = 0.001;//,hh=0.1; h = 0.0001;//,hh=0.1;
  double t0=5;     //stationary
  double t1=5.0+t0;//accelerate(speed up)
  double t2=2.0+t1;//free run
  double t3=5.0+t2;//decelerate(slow down)
  double t4=20+t3;   //free run
  initialize();
  int M=(t4/crane.h)+1;
  double *ddX=(double*)malloc(sizeof(double)*M);
  //  double *F  =(double*)malloc(sizeof(double)*M);
  double Vmax=1;//dX/dt=1[m/s]

  int n;
  double t;
  ////////////////////////////////////////////////////
  ///   set ddX                                    ///
  ////////////////////////////////////////////////////
  int n0 =t0/crane.h+0.5, n4=t4/crane.h+0.5;
  for(t=0;t<t4;t+=crane.h){
    n=t/crane.h;
    if(t<=t0) ddX[n]=0; //zero
    else if(t<=t1) ddX[n]=Vmax/(t1-t0); //accelerate
    else if(t<=t2) ddX[n]=0;            //free move
    else if(t<=t3) ddX[n]=-Vmax/(t3-t2);//decelerate
    else ddX[n]=0;                      //free move
  }
  ////////////////////////////////////////////////////
  ///   Solve by the Runge Kutta Method of GSL     ///
  ////////////////////////////////////////////////////
  fp=fopen(fn,"w");
  fprintf(fp,"#%.7e %d %d %.7e %.7e %.7e %.7e #h,n0,n4,M,n,r\n",crane.h,n0,n4,crane.M,crane.m,crane.r,crane.C);
  //  double dx0=0,dy0=0,da0=0;
  //  for(n=0;n<n4;){
  for(t=0;t<t4;t+=crane.h){
    n=t/crane.h;
    plant(ddX[n]);
  }
  fclose(fp);
  fprintf(stdout,"Results are stored in '%s'.\n",fn);

  gsl_odeiv_step_free (crane._s);
  return 0;
}

#ifndef CRANESUB
int main()
{
  ////////////////////////////////////////////////////
  /// method 1                                     ///
  /// input ddX                                    ///
  /// output a,x,y,F                               ///
  ////////////////////////////////////////////////////
  //  double h = 0.001;//,hh=0.1; h = 0.0001;//,hh=0.1;
  double t0=5;     //stationary
  double t1=5.0+t0;//accelerate(speed up)
  double t2=2.0+t1;//free run
  double t3=5.0+t2;//decelerate(slow down)
  double t4=20+t3;   //free run
  initialize();
  int M=(t4/crane.h)+1;
  double *ddX=(double*)malloc(sizeof(double)*M);
  //  double *F  =(double*)malloc(sizeof(double)*M);
  double Vmax=1;//dX/dt=1[m/s]

  int n;
  double t;
  ////////////////////////////////////////////////////
  ///   set ddX                                    ///
  ////////////////////////////////////////////////////
  int n0 =t0/crane.h+0.5, n4=t4/crane.h+0.5;
  for(t=0;t<t4;t+=crane.h){
    n=t/crane.h;
    if(t<=t0) ddX[n]=0; //zero
    else if(t<=t1) ddX[n]=Vmax/(t1-t0); //accelerate
    else if(t<=t2) ddX[n]=0;            //free move
    else if(t<=t3) ddX[n]=-Vmax/(t3-t2);//decelerate
    else ddX[n]=0;                      //free move
  }
  ////////////////////////////////////////////////////
  ///   Solve by the Runge Kutta Method of GSL     ///
  ////////////////////////////////////////////////////
  fp=fopen(fn,"w");
  fprintf(fp,"#%.7e %d %d %.7e %.7e %.7e %.7e #h,n0,n4,M,n,r\n",crane.h,n0,n4,crane.M,crane.m,crane.r,crane.C);
  //  double dx0=0,dy0=0,da0=0;
  //  for(n=0;n<n4;){
  for(t=0;t<t4;t+=crane.h){
    n=t/crane.h;
    plant(ddX[n]);
  }
  fclose(fp);
  fprintf(stdout,"Results are stored in '%s'.\n",fn);

  gsl_odeiv_step_free (crane._s);
  return 0;
}
#endif //#ifndef SUB


text:
#ifndef OPT_H
#define OPT_H
#include <gsl/gsl_vector.h>

void optimize_simplex(const gsl_vector* gamma, const gsl_vector* v, double lambda, gsl_vector* opt_x);
double f_simplex(const gsl_vector* gamma, const gsl_vector* v, double lambda, const gsl_vector* opt_x);
void df_simplex(const gsl_vector* gamma, const gsl_vector* v, double lambda, const gsl_vector* opt_x, gsl_vector* g);
bool is_feasible(const gsl_vector* x);
void simplex_projection(const gsl_vector* x, gsl_vector* x_proj, double z=1.0);

#endif // OPT_H

ext: h

max_stars_repo_name: bit-jmm/cctr

max_issues_repo_name: bit-jmm/cctr

max_forks_repo_name: bit-jmm/cctr


text:
/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C header for functions manipulating waveforms.
 *
 */

#ifndef _WAVEFORM_H
#define _WAVEFORM_H

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "EOBNRv2HMROM.h"

#if defined(__cplusplus)
extern "C" {
#define complex _Complex
#elif 0
} /* so that editors will match preceding brace */
#endif

/* NOTE: uses the list of modes of EOBNRv2HMROM (listmode), to be extended when more waveform models are added */

/***************** Function estimating frequency corresponding to a given time to coalescence ****************/

/* Functions computing relations between chirp mass, eta, m1, m2*/
double Mchirpofm1m2(
  const double m1,   /* Mass 1 */
  const double m2);  /* Mass 2 */
double etaofm1m2(
  const double m1,   /* Mass 1 */
  const double m2);  /* Mass 2 */
double m1ofMchirpeta(
  const double Mchirp,   /* Chirp mass */
  const double eta);     /* Symmetric mass ratio */
double m2ofMchirpeta(
  const double Mchirp,   /* Chirp mass */
  const double eta);     /* Symmetric mass ratio */

/* Newtonian estimate of the relation Mf(deltat/M) (for the 22 mode) - gives the starting geometric frequency for a given mass ratio and a given geometric duration of the observations */
double NewtonianfoftGeom(const double q, const double t); /* t here is t/M */
/* Newtonian estimate of the relation f(deltat) (for the 22 mode) - gives the starting frequency in Hz for a given mass ratio and a given geometric duration of the observations */
double Newtonianfoft(const double m1, const double m2, const double t); /* t here is in years, m1-m2 in solar masses */
/* Newtonian estimate of the relation f(deltat) (for the 22 mode freq) - gives the starting geometric frequency for a given time to merger and chirp mass - output in Hz */
double Newtonianfoftchirp(
  const double mchirp,                 /* Chirp mass (solar masses) */
  const double t);                     /* Time in years */
/* Newtonian estimate of the relation deltat(f) (for the 22 mode freq) - gives the time to merger from a starting frequency for a given chirp mass - output in years */
double Newtoniantoffchirp(
  const double mchirp,                 /* Chirp mass (solar masses) */
  const double t);                     /* Freq in Hz */

/***************** Function estimating time to coalescence and min/max frequency ****************/

/* Functions reading from a list of modes the minimal and maximal frequencies */
double ListmodesCAmpPhaseFrequencySeries_maxf(ListmodesCAmpPhaseFrequencySeries* listhlm);
double ListmodesCAmpPhaseFrequencySeries_minf(ListmodesCAmpPhaseFrequencySeries* listhlm);

/* Function estimating initial time from Psi22, according to tf_SPA = -1/(2pi)dPsi/df */
double EstimateInitialTime(ListmodesCAmpPhaseFrequencySeries* listhlm, double fLow);

/***************** Functions to manipulate ReImFrequencySeries structure ****************/

void ReImFrequencySeries_AddCAmpPhaseFrequencySeries(
  struct tagReImFrequencySeries* freqseriesReIm,              /* Output Re/Im frequency series */
  struct tagCAmpPhaseFrequencySeries* freqseriesCAmpPhase,    /* Input CAmp/Phase frequency series, to be interpolated and added to the output */
  double fLow,                                                /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                               /* Maximal frequency - set to 0 to ignore */
  double fstartobsmode);                                      /* Starting frequency in case of limited duration of observations- assumed to have been scaled with the proper factor m/2 for this mode - set to 0 to ignore */
/* Function evaluating a ReImFrequencySeries by interpolating wach mode of a ListmodesCAmpPhaseFrequencySeries and summing them, given a set of frequencies */
void ReImFrequencySeries_SumListmodesCAmpPhaseFrequencySeries(
  struct tagReImFrequencySeries* freqseriesReIm,                    /* Output Re/Im frequency series - already initialized */
  struct tagListmodesCAmpPhaseFrequencySeries* listmodesCAmpPhase,  /* Input CAmp/Phase frequency series, to be interpolated */
  gsl_vector* freq,                                                 /* Input set of frequencies on which evaluating */
  double fLow,                                                      /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                                     /* Maximal frequency - set to 0 to ignore */
  double fstartobs);                                                /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
/* Helper function to add a mode to hplus, hcross in Fourier domain
 * - copies the function XLALSimAddMode, which was done only for TD structures */
int FDAddMode(
  ReImFrequencySeries* hptilde,       /* Output: frequency series for hplus */
  ReImFrequencySeries* hctilde,       /* Output: frequency series for hcross */
  ReImFrequencySeries* hlmtilde,      /* Input: frequency series for the mode hlm */
  double theta,                       /* First angle for position in the sky of observer */
  double phi,                         /* Second angle for position in the sky of observer  */
  int l,                              /* First mode number l */
  int m,                              /* Second mode number m */
  int sym);                           /* If 1, assume planar symmetry and add also mode l,-m. Do not if set to 0. */
/* Function evaluating the FD frequency series for hplus, hcross from the modes hlm */
int GeneratehphcFDReImFrequencySeries(
  ReImFrequencySeries** hptilde,                 /* Output: frequency series for hplus */
  ReImFrequencySeries** hctilde,                 /* Output: frequency series for hcross */
  ListmodesCAmpPhaseFrequencySeries* listhlm,    /* Input: frequency series for the mode hlm  */
  double fLow,                                   /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                  /* Maximal frequency - set to 0 to ignore */
  double fstartobs,                              /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
  double deltaf,                                 /* Frequency step */
  int nbpt,                                      /* Number of points of output - if 0, determined from deltaF and maximal frequency in input */
  int nbmode,                                    /* Number of modes to add */
  double theta,                                  /* First angle for position in the sky of observer */
  double phi,                                    /* Second angle for position in the sky of observer */
  int sym);                                      /* If 1, assume planar symmetry and add also mode l,-m. Do not if set to 0. */
/* Function evaluating the FD frequency series by summing mode contributions from each hlm */
int GenerateFDReImFrequencySeries(
  ReImFrequencySeries** freqseries,              /* Output: frequency series */
  ListmodesCAmpPhaseFrequencySeries* listhlm,    /* Input: FD modes hlm in the form AmpReal/AmpIm/Phase  */
  double fLow,                                   /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                  /* Maximal frequency - set to 0 to ignore */
  double fstartobs,                              /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
  double deltaf,                                 /* Frequency step */
  int nbpt);                                     /* Number of points of output - if 0, determined from deltaf and maximal frequency in input */
/* Function evaluating the FD frequency series for a single mode contribution (l,m) */
int GenerateFDReImFrequencySeriesSingleMode(
  ReImFrequencySeries** freqseries,              /* Output: frequency series */
  ListmodesCAmpPhaseFrequencySeries* listhlm,    /* Input: FD modes hlm in the form AmpReal/AmpIm/Phase  */
  double fLow,                                   /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                  /* Maximal frequency - set to 0 to ignore */
  double fstartobs,                              /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
  double deltaf,                                 /* Frequency step */
  int nbpt,                                      /* Number of points of output - if 0, determined from deltaf and maximal frequency in input */
  int l,                                         /* Mode index l */
  int m);                                        /* Mode index m */
/* Function to restrict a frequency series (typically output of a FFT) to a given frequency range */
int RestrictFDReImFrequencySeries(
  ReImFrequencySeries** freqseriesout,           /* Output: truncated frequency series */
  ReImFrequencySeries* freqseriesin,             /* Input: frequency series */
  double fLow,                                   /* Minimal frequency */
  double fHigh);                                 /* Maximal frequency */
/* Function for getting a phase mod 2pi (between -pi and pi) */
double mod2pi(double phase);
/* Function for getting a phase mod pi (between 0 and pi, e.g. polarization) */
double modpi(double phase);
/* Function to unwrap the phase mod 2pi  - acts directly on the gsl_vector representing the phase */
int UnwrapPhase(
  gsl_vector*  phaseout,   /* Output: unwrapped phase vector - already allocated */
  gsl_vector*  phasein);   /* Input: phase vector */
/* Function to convert a time series from Re/Im form to Amp/Phase form - unwrapping the phase */
int ReImTimeSeries_ToAmpPhase(
  AmpPhaseTimeSeries** timeseriesout,             /* Output: Amp/Phase time series */
  ReImTimeSeries* timeseriesin);                  /* Input: Re/Im time series */
/* Function to convert a time series from Amp/Phase form to Re/Im form */
int AmpPhaseTimeSeries_ToReIm(
  ReImTimeSeries** timeseriesout,                 /* Output: Re/Im time series */
  AmpPhaseTimeSeries* timeseriesin);              /* Input: Amp/Phase time series */
/* Function to compute a linear resampling at high frequencies to enforce a maximal deltaf */
/* NOTE: Assumes input frequencies are logarithmic (except maybe first interval) to evaluate when to resample */
int SetMaxdeltafResampledFrequencies(
  gsl_vector** freqr,              /* Output: resampled frequencies */
  gsl_vector* freq,                /* Input: original frequencies */
  const double maxf,               /* Input: maximal frequency - set to 0. to ignore */
  const double deltaf);            /* Input: maximal deltaf aimed for - 0.002Hz appropriate for LISA */
/* Function to compute a linear-in-time resampling at low frequencies to enforce a maximal deltat */
int SetMaxdeltatResampledFrequencies(
  gsl_vector** freqr,              /* Output: resampled frequencies */
  gsl_vector* freq,                /* Input: original frequencies */
  const double deltat,             /* Input: maximal deltat aimed for - fraction of a year, 1/24 (half month) appropriate for 1e-4 interpolation errors */
  const double mchirp,             /* Input: chirp mass, used for approximate t-f correspondence */
  const int m);                    /* Input: chirp mass, used for approximate t-f correspondence */
/* Function to resample a CAmp/Phase frequency series on the specified frequencies */
int CAmpPhaseFrequencySeries_Resample(
  CAmpPhaseFrequencySeries** freqseriesout,         /* Output: CAmp/Phase freq series */
  CAmpPhaseFrequencySeries* freqseriesin,           /* Input: CAmp/Phase freq series */
  gsl_vector* freqr);                                 /* Input: freq vector to resample on */

/***************** Spin weighted spherical harmonics ****************/

/* Additional function reproducing XLALSpinWeightedSphericalHarmonic */
double complex SpinWeightedSphericalHarmonic(double theta, double phi, int s, int l, int m); /* Currently only supports s=-2, l=2,3,4,5 modes */

#if 0
{ /* so that editors will match succeeding brace */
#elif defined(__cplusplus)
}
#endif

#endif /* _WAVEFORM_H */

max_stars_repo_name: JohnGBaker/flare

max_issues_repo_name: JohnGBaker/flare

max_forks_repo_name: JohnGBaker/flare


text:
/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 */

#pragma once
#ifndef COMMON_H
#define COMMON_H

// Since we call cblas_dgemm in openmp for loop,
// we call "extension" APIs for setting the number of threads.
#ifdef USE_INTEL_MKL
#include <mkl.h>
	#if INTEL_MKL_VERSION < 20170000
		// Will throw an error at development time in non-standard settings
		PLEASE DONOT COMPILE SHARED LIBRARIES WITH OLDER MKL VERSIONS
	#endif
#include <mkl_service.h>
extern "C" void mkl_set_num_threads(int numThreads);
#else
#include <cblas.h>
extern "C" void openblas_set_num_threads(int numThreads);
#endif

template<class FP>
size_t computeNNZ(FP* arr, int limit) {
    size_t nnz = 0;
#ifndef USE_INTEL_MKL
#pragma omp parallel for reduction(+: nnz)
#endif
    for(int i=0; i<limit; i++)
        nnz += (arr[i]!=0) ? 1 : 0;
    return nnz;
}

static int SYSDS_CURRENT_NUM_THREADS = -1;
static void setNumThreadsForBLAS(int numThreads) {
	if (SYSDS_CURRENT_NUM_THREADS != numThreads) {
#ifdef USE_OPEN_BLAS
		openblas_set_num_threads(numThreads);
#else
		mkl_set_num_threads(numThreads);
#endif
		SYSDS_CURRENT_NUM_THREADS = numThreads;
	}
}

#endif // COMMON_H

max_stars_repo_name: Shafaq-Siddiqi/systemml


text:
/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "xform.h"
#include "imgfeatures.h"
#include "utils.h"

//#include "cxcore.h"
//#include <highgui.h>
// Universal include for all versions of OpenCV
#include <mrpt/otherlibs/do_opencv_includes.h>

#include <gsl/gsl_sf.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include <time.h>

/************************* Local Function Prototypes *************************/

static __inline struct feature* get_match( struct feature*, int );
int get_matched_features( struct feature*, int, int, struct feature*** );
int calc_min_inliers( int, int, double, double );
struct feature** draw_ransac_sample( struct feature**, int, int, gsl_rng* );
void extract_corresp_pts( struct feature**, int, int, CvPoint2D64f**,
						 CvPoint2D64f** );
int find_consensus( struct feature**, int, int, CvMat*, ransac_err_fn,
				   double, struct feature*** );
static __inline void release_mem( CvPoint2D64f*, CvPoint2D64f*,
struct feature** );

/********************** Functions prototyped in xform.h **********************/

/*
Calculates a best-fit image transform from image feature correspondences
using RANSAC.

For more information refer to:

Fischler, M. A. and Bolles, R. C.  Random sample consensus: a paradigm for
model fitting with applications to image analysis and automated cartography.
Communications of the ACM, 24, 6 (1981), pp. 381--395.

@param features an array of features; only features with a non-NULL match
	of type mtype are used in homography computation
@param n number of features in feat
@param mtype determines which of each feature's match fields to use
	for model computation; should be one of FEATURE_FWD_MATCH,
	FEATURE_BCK_MATCH, or FEATURE_MDL_MATCH; if this is FEATURE_MDL_MATCH,
	correspondences are assumed to be between a feature's img_pt field
	and its match's mdl_pt field, otherwise correspondences are assumed to
	be between the the feature's img_pt field and its match's img_pt field
@param xform_fn pointer to the function used to compute the desired
	transformation from feature correspondences
@param m minimum number of correspondences necessary to instantiate the
	model computed by xform_fn
@param p_badxform desired probability that the final transformation
	returned by RANSAC is corrupted by outliers (i.e. the probability that
	no samples of all inliers were drawn)
@param err_fn pointer to the function used to compute a measure of error
	between putative correspondences and a computed model
@param err_tol correspondences within this distance of a computed model are
	considered as inliers
@param inliers if not NULL, output as an array of pointers to the final
	set of inliers
@param n_in if not NULL and \a inliers is not NULL, output as the final
	number of inliers

@return Returns a transformation matrix computed using RANSAC or NULL
	on error or if an acceptable transform could not be computed.
*/
CvMat* ransac_xform( struct feature* features, int n, int mtype,
					ransac_xform_fn xform_fn, int m, double p_badxform,
					ransac_err_fn err_fn, double err_tol,
struct feature*** inliers, int* n_in )
{
	struct feature** matched, ** sample, ** consensus, ** consensus_max = NULL;
	struct ransac_data* rdata;
	CvPoint2D64f* pts, * mpts;
	CvMat* M = NULL;
	gsl_rng* rng;
	double p, in_frac = RANSAC_INLIER_FRAC_EST;
	int i, nm, in, in_min, in_max = 0, k = 0;

	nm = get_matched_features( features, n, mtype, &matched );
	if( nm < m )
	{
		fprintf( stderr, "Warning: not enough matches to compute xform, %s" \
			" line %d\n", __FILE__, __LINE__ );
		goto end;
	}

	/* initialize random number generator */
	rng = gsl_rng_alloc( gsl_rng_mt19937 );
	gsl_rng_set( rng, time(NULL) );

	in_min = calc_min_inliers( nm, m, RANSAC_PROB_BAD_SUPP, p_badxform );
	p = pow( 1.0 - pow( in_frac, m ), k );
	i = 0;
	while( p > p_badxform )
	{
		sample = draw_ransac_sample( matched, nm, m, rng );
		extract_corresp_pts( sample, m, mtype, &pts, &mpts );
		M = xform_fn( pts, mpts, m );
		if( ! M )
			goto iteration_end;
		in = find_consensus( matched, nm, mtype, M, err_fn, err_tol, &consensus);
		if( in > in_max )
		{
			if( consensus_max )
				free( consensus_max );
			consensus_max = consensus;
			in_max = in;
			in_frac = (double)in_max / nm;
		}
		else
			free( consensus );
		cvReleaseMat( &M );

iteration_end:
		release_mem( pts, mpts, sample );
		p = pow( 1.0 - pow( in_frac, m ), ++k );
	}

	/* calculate final transform based on best consensus set */
	if( in_max >= in_min )
	{
		extract_corresp_pts( consensus_max, in_max, mtype, &pts, &mpts );
		M = xform_fn( pts, mpts, in_max );
		in = find_consensus( matched, nm, mtype, M, err_fn, err_tol, &consensus);
		cvReleaseMat( &M );
		release_mem( pts, mpts, consensus_max );
		extract_corresp_pts( consensus, in, mtype, &pts, &mpts );
		M = xform_fn( pts, mpts, in );
		if( inliers )
		{
			*inliers = consensus;
			consensus = NULL;
		}
		if( n_in )
			*n_in = in;
		release_mem( pts, mpts, consensus );
	}
	else if( consensus_max )
	{
		if( inliers )
			*inliers = NULL;
		if( n_in )
			*n_in = 0;
		free( consensus_max );
	}

	gsl_rng_free( rng );
end:
	for( i = 0; i < nm; i++ )
	{
		rdata = feat_ransac_data( matched[i] );
		matched[i]->feature_data = rdata->orig_feat_data;
		free( rdata );
	}
	free( matched );
	return M;
}

/*
Calculates a least-squares planar homography from point correspondeces.

@param pts array of points
@param mpts array of corresponding points; each pts[i], i=0..n-1, corresponds
	to mpts[i]
@param n number of points in both pts and mpts; must be at least 4

@return Returns the 3 x 3 least-squares planar homography matrix that
	transforms points in pts to their corresponding points in mpts or NULL if
	fewer than 4 correspondences were provided
*/
CvMat* lsq_homog( CvPoint2D64f* pts, CvPoint2D64f* mpts, int n )
{
	CvMat* H, * A, * B, X;
	double x[9];
	int i;

	if( n < 4 )
	{
		fprintf( stderr, "Warning: too few points in lsq_homog(), %s line %d\n",
			__FILE__, __LINE__ );
		return NULL;
	}

	/* set up matrices so we can unstack homography into X; AX = B */
	A = cvCreateMat( 2*n, 8, CV_64FC1 );
	B = cvCreateMat( 2*n, 1, CV_64FC1 );
	X = cvMat( 8, 1, CV_64FC1, x );
	H = cvCreateMat(3, 3, CV_64FC1);
	cvZero( A );
	for( i = 0; i < n; i++ )
	{
		cvmSet( A, i, 0, pts[i].x );
		cvmSet( A, i+n, 3, pts[i].x );
		cvmSet( A, i, 1, pts[i].y );
		cvmSet( A, i+n, 4, pts[i].y );
		cvmSet( A, i, 2, 1.0 );
		cvmSet( A, i+n, 5, 1.0 );
		cvmSet( A, i, 6, -pts[i].x * mpts[i].x );
		cvmSet( A, i, 7, -pts[i].y * mpts[i].x );
		cvmSet( A, i+n, 6, -pts[i].x * mpts[i].y );
		cvmSet( A, i+n, 7, -pts[i].y * mpts[i].y );
		cvmSet( B, i, 0, mpts[i].x );
		cvmSet( B, i+n, 0, mpts[i].y );
	}
	cvSolve( A, B, &X, CV_SVD );
	x[8] = 1.0;
	X = cvMat( 3, 3, CV_64FC1, x );
	cvConvert( &X, H );

	cvReleaseMat( &A );
	cvReleaseMat( &B );
	return H;
}

/*
Calculates the transfer error between a point and its correspondence for
a given homography, i.e. for a point x, it's correspondence x', and
homography H, computes d(x', Hx)^2.

@param pt a point
@param mpt pt's correspondence
@param H a homography matrix

@return Returns the transfer error between pt and mpt given H
*/
double homog_xfer_err( CvPoint2D64f pt, CvPoint2D64f mpt, CvMat* H )
{
	CvPoint2D64f xpt = persp_xform_pt( pt, H );

	return sqrt( dist_sq_2D( xpt, mpt ) );
}

/*
Performs a perspective transformation on a single point.  That is, for a
point (x, y) and a 3 x 3 matrix T this function returns the point
(u, v), where

[x' y' w']^T = T * [x y 1]^T,

and

(u, v) = (x'/w', y'/w').

Note that affine transforms are a subset of perspective transforms.

@param pt a 2D point
@param T a perspective transformation matrix

@return Returns the point (u, v) as above.
*/
CvPoint2D64f persp_xform_pt( CvPoint2D64f pt, CvMat* T )
{
	CvMat XY, UV;
	double xy[3] = { pt.x, pt.y, 1.0 }, uv[3] = { 0 };
	CvPoint2D64f rslt;

	cvInitMatHeader( &XY, 3, 1, CV_64FC1, xy, CV_AUTOSTEP );
	cvInitMatHeader( &UV, 3, 1, CV_64FC1, uv, CV_AUTOSTEP );
	cvMatMul( T, &XY, &UV );
	rslt = cvPoint2D64f( uv[0] / uv[2], uv[1] / uv[2] );

	return rslt;
}

/************************ Local funciton definitions *************************/

/*
Returns a feature's match according to a specified match type

@param feat feature
@param mtype match type, one of FEATURE_FWD_MATCH, FEATURE_BCK_MATCH, or
FEATURE_MDL_MATCH

@return Returns feat's match corresponding to mtype or NULL for bad mtype
*/
static __inline struct feature* get_match( struct feature* feat, int mtype )
{
	if( mtype == FEATURE_MDL_MATCH )
		return feat->mdl_match;
	if( mtype == FEATURE_BCK_MATCH )
		return feat->bck_match;
	if( mtype == FEATURE_FWD_MATCH )
		return feat->fwd_match;
	return NULL;
}

/*
Finds all features with a match of a specified type and stores pointers
to them in an array.  Additionally initializes each matched feature's
feature_data field with a ransac_data structure.

@param features array of features
@param n number of features in features
@param mtype match type, one of FEATURE_{FWD,BCK,MDL}_MATCH
@param matched output as an array of pointers to features with a match of
the specified type

@return Returns the number of features output in matched.
*/
int get_matched_features( struct feature* features, int n, int mtype,
struct feature*** matched )
{
	struct feature** _matched;
	struct ransac_data* rdata;
	int i, m = 0;

	_matched = calloc( n, sizeof( struct feature* ) );
	for( i = 0; i < n; i++ )
		if( get_match( features + i, mtype ) )
		{
			rdata = malloc( sizeof( struct ransac_data ) );
			memset( rdata, 0, sizeof( struct ransac_data ) );
			rdata->orig_feat_data = features[i].feature_data;
			_matched[m] = features + i;
			_matched[m]->feature_data = rdata;
			m++;
		}
		*matched = _matched;
		return m;
}

/*
Calculates the minimum number of inliers as a function of the number of
putative correspondences.  Based on equation (7) in

Chum, O. and Matas, J.  Matching with PROSAC -- Progressive Sample Consensus.
In Conference on Computer Vision and Pattern Recognition (CVPR),
(2005), pp. 220--226.

@param n number of putative correspondences
@param m min number of correspondences to compute the model in question
@param p_badsupp prob. that a bad model is supported by a correspondence
@param p_badxform desired prob. that the final transformation returned is bad

@return Returns the minimum number of inliers required to guarantee, based
	on p_badsupp, that the probability that the final transformation returned
	by RANSAC is less than p_badxform
*/
int calc_min_inliers( int n, int m, double p_badsupp, double p_badxform )
{
	double pi, sum;
	int i, j;

	for( j = m+1; j <= n; j++ )
	{
		sum = 0;
		for( i = j; i <= n; i++ )
		{
			pi = ( i - m ) * log( p_badsupp ) + ( n - i + m ) * log( 1.0 - p_badsupp ) +
				gsl_sf_lnchoose( n - m, i - m );
			sum += exp( pi );
		}
		if( sum < p_badxform )
			break;
	}
	return j;
}

/*
Draws a RANSAC sample from a set of features.

@param features array of pointers to features from which to sample
@param n number of features in features
@param m size of the sample
@param rng random number generator used to sample

@return Returns an array of pointers to the sampled features; the sampled
	field of each sampled feature's ransac_data is set to 1
*/
struct feature** draw_ransac_sample( struct feature** features, int n,
									int m, gsl_rng* rng )
{
	struct feature** sample, * feat;
	struct ransac_data* rdata;
	int i, x;

	for( i = 0; i < n; i++ )
	{
		rdata = feat_ransac_data( features[i] );
		rdata->sampled = 0;
	}

	sample = calloc( m, sizeof( struct feature* ) );
	for( i = 0; i < m; i++ )
	{
		do
		{
			x = gsl_rng_uniform_int( rng, n );
			feat = features[x];
			rdata = feat_ransac_data( feat );
		}
		while( rdata->sampled );
		sample[i] = feat;
		rdata->sampled = 1;
	}

	return sample;
}

/*
Extrancs raw point correspondence locations from a set of features

@param features array of features from which to extract points and match
	points; each of these is assumed to have a match of type mtype
@param n number of features
@param mtype match type; if FEATURE_MDL_MATCH correspondences are assumed
	to be between each feature's img_pt field and it's match's mdl_pt field,
	otherwise, correspondences are assumed to be between img_pt and img_pt
@param pts output as an array of raw point locations from features
@param mpts output as an array of raw point locations from features' matches
*/
void extract_corresp_pts( struct feature** features, int n, int mtype,
						 CvPoint2D64f** pts, CvPoint2D64f** mpts )
{
	struct feature* match;
	CvPoint2D64f* _pts, * _mpts;
	int i;

	_pts = calloc( n, sizeof( CvPoint2D64f ) );
	_mpts = calloc( n, sizeof( CvPoint2D64f ) );

	if( mtype == FEATURE_MDL_MATCH )
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			_pts[i] = features[i]->img_pt;
			_mpts[i] = match->mdl_pt;
		}

	else
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			_pts[i] = features[i]->img_pt;
			_mpts[i] = match->img_pt;
		}

		*pts = _pts;
		*mpts = _mpts;
}

/*
For a given model and error function, finds a consensus from a set of
feature correspondences.

@param features set of pointers to features; every feature is assumed to
	have a match of type mtype
@param n number of features in features
@param mtype determines the match field of each feature against which to
	measure error; if this is FEATURE_MDL_MATCH, correspondences are assumed
	to be between the feature's img_pt field and the match's mdl_pt field;
	otherwise matches are assumed to be between img_pt and img_pt
@param M model for which a consensus set is being found
@param err_fn error function used to measure distance from M
@param err_tol correspondences within this distance of M are added to the
	consensus set
@param consensus output as an array of pointers to features in the
	consensus set

@return Returns the number of points in the consensus set
*/
int find_consensus( struct feature** features, int n, int mtype,
				   CvMat* M, ransac_err_fn err_fn, double err_tol,
				   struct feature*** consensus )
{
	struct feature** _consensus;
	struct feature* match;
	CvPoint2D64f pt, mpt;
	double err;
	int i, in = 0;

	_consensus = calloc( n, sizeof( struct feature* ) );

	if( mtype == FEATURE_MDL_MATCH )
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			pt = features[i]->img_pt;
			mpt = match->mdl_pt;
			err = err_fn( pt, mpt, M );
			if( err <= err_tol )
				_consensus[in++] = features[i];
		}

	else
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			pt = features[i]->img_pt;
			mpt = match->img_pt;
			err = err_fn( pt, mpt, M );
			if( err <= err_tol )
				_consensus[in++] = features[i];
		}
	*consensus = _consensus;
	return in;
}

/*
Releases memory and reduces code size above

@param pts1 an array of points
@param pts2 an array of points
@param features an array of pointers to features; can be NULL
*/
static __inline void release_mem( CvPoint2D64f* pts1, CvPoint2D64f* pts2,
struct feature** features )
{
	free( pts1 );
	free( pts2 );
	if( features )
		free( features );
}


text:
#ifndef _COMMON_
#define _COMMON_

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_roots.h>
#include "params.h"
//#include "dam_utils.h"
#include "cosmo_mad.h"

#define DTOR 0.01745329251

typedef struct {
  gsl_interp_accel *intacc;
  gsl_spline *spline;
  double x0,xf;
  double y0,yf;
} SplPar;

typedef struct {
  double om,ol,ob;
  double w0,wa,h0;
  double ns,s8;
  char **fname_window;
  char fname_bias[256];
  char fname_sbias[256];
  char fname_pk[256];
  char prefix_out[256];
  int lmax;
  Csm_params *cpar;
  double chi_horizon;
  double chi_LSS;
  double prefac_lensing;
  double dchi;
  int do_nc;
  int do_shear;
  int do_cmblens;
  int do_isw;
  int has_bg;
  int has_dens;
  int has_rsd;
  int has_lensing;
  SplPar *aofchi;
  SplPar *zofchi;
  SplPar *hofchi;
  SplPar *gfofchi;
  SplPar *fgofchi;
  SplPar **wind_0;
  SplPar **wind_M;
  SplPar **wind_L;
  SplPar *bias;
  SplPar *sbias;
  double *cl_dd;
  double *cl_d1l2;
  double *cl_d2l1;
  double *cl_dc;
  double *cl_di;
  double *cl_ll;
  double *cl_lc;
  double *cl_li;
  double *cl_cc;
  double *cl_ci;
  double *cl_ii;
  int do_w_theta;
  int do_w_theta_logbin;
  double th_min;
  double th_max;
  int n_th;
  int n_th_logint;
  double *wt_dd;
  double *wt_d1l2;
  double *wt_d2l1;
  double *wt_dc;
  double *wt_di;
  double *wt_ll_pp;
  double *wt_ll_mm;
  double *wt_lc;
  double *wt_li;
  double *wt_cc;
  double *wt_ci;
  double *wt_ii;
} RunParams;

//Defined in common.c
void dam_report_error(int level,char *fmt,...);
void *dam_malloc(size_t size);
void *dam_calloc(size_t nmemb,size_t size);
FILE *dam_fopen(const char *path,const char *mode);
int dam_linecount(FILE *f);
SplPar *spline_init(int n,double *x,double *y,double y0,double yf);
double spline_eval(double x,SplPar *spl);
void spline_free(SplPar *spl);
RunParams *param_new(void);
void param_free(RunParams *par);

//Defined in cosmo.c
RunParams *init_params(char *fname_ini);

//Defined in transfers.c
double transfer_wrap(int l,double k,RunParams *par,char *trtype,int ibin);

//Defined in spectra.c
void compute_spectra(RunParams *par);
void compute_w_theta(RunParams *par);

//Defined in io.c
int read_parameter_file(char *fname,RunParams *par);
void write_output(RunParams *par);

#endif //_COMMON_

max_stars_repo_name: Jappenn/CCL

max_issues_repo_name: Jappenn/CCL

max_forks_repo_name: Jappenn/CCL


text:
// Authors: David Blei (blei@cs.princeton.edu)
//          Sean Gerrish (sgerrish@cs.princeton.edu)
//
// Copyright 2011 Sean Gerrish and David Blei
// All Rights Reserved.
//
// See the README for this package for details about modifying or
// distributing this software.

#include "lda-seq.h"

#include "gflags.h"
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector_double.h>

extern int LDA_INFERENCE_MAX_ITER;

static double* scaled_influence = NULL;

// const int TIME = 61;
const int TIME = -3;
const double PI = 3.141592654;

DEFINE_int32(lda_sequence_max_iter,
	     20,
	     "The maximum number of iterations.");
DEFINE_int32(lda_sequence_min_iter,
	     1,
	     "The maximum number of iterations.");

DEFINE_int32(forward_window,
	     1,
	     "The forward window for deltas. "
	     "If negative, we use a beta with mean "
	     "5.");

DEFINE_string(normalize_docs,
	      "normalize",
	      "Describes how documents's wordcounts "
	      "are considered for finding influence. "
	      "Options are \"normalize\", \"none\", "
	      "\"occurrence\", \"log\", or \"log_norm\".");

DEFINE_int32(save_time,
	     1e20,
	     "Save a specific time.  If -1, save all times.");

DEFINE_int32(fix_topics,
	     0,
	     "Fix a set of this many topics. This amounts "
	     "to fixing these topics' variance at 1e-10.");

DECLARE_string(model);
DECLARE_int32(max_number_time_points);
DECLARE_double(sigma_d);
DECLARE_double(sigma_l);
DECLARE_double(sigma_c);
DECLARE_double(sigma_cv);

/*
 * populate an LDA model at a particular time point
 *
 */

inf_var* inf_var_alloc(int number_topics,
		       corpus_seq_t* corpus_seq) {
  // Hate to do this, but I had trouble using it before.  This should
  // be the first place we use it; otherwise we'll get a sigsev nil.
  if (scaled_influence == NULL) {
    scaled_influence = NewScaledInfluence(FLAGS_max_number_time_points);
  }

  inf_var* inf_var_ptr = (inf_var*) malloc(sizeof(inf_var));
  inf_var_ptr->doc_weights = (gsl_matrix**) malloc(sizeof(gsl_matrix*)
						   * corpus_seq->len);
  inf_var_ptr->renormalized_doc_weights = (gsl_matrix**) malloc(
    sizeof(gsl_matrix*)
    * corpus_seq->len);
  inf_var_ptr->ntime = corpus_seq->len;
  int i=0;
  for (i=0; i < corpus_seq->len; ++i) {
    corpus_t* corpus = corpus_seq->corpus[i];
    outlog("creating matrix. %d %d", corpus->ndocs, number_topics);
    if (corpus->ndocs == 0) {
      inf_var_ptr->doc_weights[i] = (gsl_matrix*) malloc(sizeof(gsl_matrix));
      inf_var_ptr->doc_weights[i]->size1 = 0;
      inf_var_ptr->doc_weights[i]->size2 = number_topics;
      inf_var_ptr->renormalized_doc_weights[i] = (gsl_matrix*) malloc(sizeof(gsl_matrix));
      inf_var_ptr->renormalized_doc_weights[i]->size1 = 0;
      inf_var_ptr->renormalized_doc_weights[i]->size2 = number_topics;
    } else {
      inf_var_ptr->doc_weights[i] = gsl_matrix_calloc(corpus->ndocs,
						      number_topics);
      inf_var_ptr->renormalized_doc_weights[i] = gsl_matrix_calloc(corpus->ndocs,
								   number_topics);
    }
  }
  return inf_var_ptr;
}

void inf_var_free(inf_var* ptr) {
  // TODO.
}

// Solves the linear system Ax = b for x.
// Assumes that x is already allocated.
void Solve(gsl_matrix* A,
	   const gsl_vector* b,
	   gsl_vector* x) {
    int permutation_sign;
    gsl_permutation* permutation = gsl_permutation_alloc(b->size);
    gsl_linalg_LU_decomp(A, permutation, &permutation_sign);
    gsl_linalg_LU_solve(A, permutation, b, x);
    gsl_permutation_free(permutation);
}

// Find the sums of influence of all documents in advance.
// g has scores for everything *up to but not including* g.
void InfluenceTotalFixed(lda_seq* seq,
		    const corpus_seq_t* data) {
  gsl_vector* exp_tmp = gsl_vector_alloc(seq->nterms);

  for (int k=0; k < seq->ntopics; ++k) {

    for (int s=0; s < seq->nseq; ++s) {
      // Pull out elements of g, and make sure to set them to 0!
      gsl_vector_view g = gsl_matrix_column(
	  seq->influence_sum_lgl[k], s);
      gsl_vector_set_zero(&g.vector);

      for (int t=0; t <= s; ++t) {
	gsl_vector_view w_phi_l =
	  gsl_matrix_column(seq->topic[k]->w_phi_l, t);
	gsl_vector_memcpy(exp_tmp, &w_phi_l.vector);
	gsl_vector_scale(exp_tmp, scaled_influence[s - t]);
	gsl_vector_add(&g.vector, exp_tmp);
      }
    }
  }
  gsl_vector_free(exp_tmp);
}

void DumpTimeDocTopicStats(const char* root,
			   size_t t,
			   corpus_t* corpus,
			   gsl_matrix** phi) {
  // For all documents, dump the top topics.

  char name[400];
  // Dump the top topics for each word.
  sprintf(name, "%s%ld_doc_term_topics.dat", root, t);
  FILE* f = fopen(name, "w");
  for (unsigned int d=0; d < corpus->ndocs; ++d) {
    gsl_matrix* phi_d = phi[d];
    doc_t* doc = corpus->doc[d];
    for (unsigned int n=0; n < doc->nterms; ++n) {

      unsigned int w = doc->word[n];

      // First, find the max topic weight.
      unsigned int max_topic_index = 0;
      double max_topic_weight = gsl_matrix_get(phi_d, n, 0);
      for (unsigned int k=0; k < phi_d->size2; ++k) {
	double phi_d_n_k = gsl_matrix_get(phi_d, n, k);
	if (phi_d_n_k > max_topic_weight) {
	  max_topic_weight = phi_d_n_k;
	  max_topic_index = k;
	}
      }

      fprintf(f, "%d:%d:%.3f", w, max_topic_index, max_topic_weight);

      if (n < doc->nterms - 1) {
	fprintf(f, " ");
      }
    }
    fprintf(f, "\n");
  }
  fclose(f);
}

void PrepareRegressionComponents(
    corpus_t* corpus,
    lda_seq* seq,
    unsigned int k,
    gsl_matrix* W_phi,
    gsl_matrix* W_phi_var,
    gsl_vector* d_phi_var_tmp,
    gsl_vector* response,
    gsl_vector* d_tmp,
    gsl_matrix* dd_tmp,
    gsl_vector* document_weights,
    gsl_matrix* W_phi_tmp,
    gsl_vector* exp_h_tmp) {

  // Then left-multiply this by W_phi:
  gsl_blas_dgemv(CblasTrans,
		 1.0,
		 W_phi,
		 response,
		 0.0,
		 d_tmp);

  // Set up the transformation matrix.
  // First, set up W_phi^T \Lambda W_phi.
  gsl_matrix_memcpy(W_phi_tmp, W_phi);
  for (unsigned int d=0; d < corpus->ndocs; ++d) {
    gsl_vector_view col = gsl_matrix_column(W_phi_tmp, d);
    gsl_vector_mul(&col.vector, exp_h_tmp);
  }

  // Yuck.  Maybe we should do this sparsely?
  // Probably won't be too bad, at least for now.
  gsl_blas_dgemm(CblasTrans,
		 CblasNoTrans,
		 1.0,
		 W_phi_tmp,
		 W_phi,
		 0.0,
		 dd_tmp);

  gsl_blas_dgemv(CblasTrans,
		 1.0,
		 W_phi_var,
		 exp_h_tmp,
		 0.0,
		 d_phi_var_tmp);

  // Next, add elements to the diagonal of dd_tmp.
  for (unsigned int d=0; d < corpus->ndocs; ++d) {
    double value = gsl_matrix_get(dd_tmp, d, d);
    value += (seq->topic[k]->chain_variance
	      / (FLAGS_sigma_d * FLAGS_sigma_d));
    // sgerrish: Is this supposed to be multiplied by anything?
    value += gsl_vector_get(d_phi_var_tmp, d);
    gsl_matrix_set(dd_tmp, d, d, value);
  }
}

void SetExpHTmp(lda_seq* seq,
		const corpus_seq_t* data,
		unsigned int t,
		unsigned int k,
		unsigned int n,
		gsl_vector* exp_h_tmp,
		gsl_vector* zw_tmp,
		gsl_vector** exp_i_tmp) {
  gsl_vector_set_zero(exp_h_tmp);
  for (int i = t; i < seq->nseq; ++i) {
    gsl_vector_view mean_i_current =
      gsl_matrix_column(seq->topic[k]->e_log_prob, i);
    gsl_vector_view var_i_current =
      gsl_matrix_column(seq->topic[k]->variance, i + 1);

    // Set up exp_h_tmp.
    gsl_vector_memcpy(zw_tmp, &var_i_current.vector);
    gsl_vector_sub(zw_tmp, &mean_i_current.vector);
    gsl_vector_scale(zw_tmp, 2.0);
    for (n=0; n < data->nterms; ++n) {
      gsl_vector_set(zw_tmp, n,
		     exp(gsl_vector_get(zw_tmp, n)));
    }
    gsl_vector_scale(zw_tmp,
		     scaled_influence[i - t]
		     * scaled_influence[i - t]);
    gsl_vector_add(exp_h_tmp, zw_tmp);

    // Set up exp_i_tmp.
    gsl_vector_memcpy(exp_i_tmp[i], &var_i_current.vector);
    gsl_vector_scale(exp_i_tmp[i], 0.5);
    gsl_vector_sub(exp_i_tmp[i], &mean_i_current.vector);
    for (n=0; n < data->nterms; ++n) {
      gsl_vector_set(exp_i_tmp[i], n,
		     exp(gsl_vector_get(exp_i_tmp[i], n)));
    }
  }
}

double update_inf_var_fixed(lda_seq* seq,
			    const corpus_seq_t* data,
			    gsl_matrix** phi,
			    size_t t,
			    const char* root,
			    int dump_doc_stats) {
  double lhood = 0.0;
  // Note that we're missing a suspicious factor of -2 on the document
  // weights currently.  We won't worry about that for now (since
  // we're still experimenting), but it should soon be fixed.
  corpus_t* corpus = data->corpus[t];
  if (!corpus->ndocs) {
    return lhood;
  }
  inf_var* influence = seq->influence;
  if (t != TIME && 0) {
    return lhood;
  }

  // We need access to the following:
  gsl_matrix* documents_topics_t = influence->doc_weights[t];
  gsl_matrix* renormalized_documents_topics_t = influence->renormalized_doc_weights[t];

  size_t k;

  gsl_matrix* W_phi = gsl_matrix_calloc(
    seq->nterms, corpus->ndocs);
  gsl_matrix* W2_phi2 = gsl_matrix_calloc(
    seq->nterms, corpus->ndocs);
  gsl_matrix* W_phi_var =
    gsl_matrix_calloc(seq->nterms, corpus->ndocs);
  gsl_matrix* W_phi_tmp = gsl_matrix_alloc(
    seq->nterms, corpus->ndocs);
  gsl_matrix* dd_tmp = gsl_matrix_alloc(
    corpus->ndocs, corpus->ndocs);
  gsl_vector* xd_tmp = gsl_vector_calloc(
    corpus->ndocs);
  gsl_vector* yw_tmp = gsl_vector_calloc(
    seq->nterms);
  gsl_vector* zw_tmp = gsl_vector_calloc(
    seq->nterms);
  gsl_vector* terms_inc_tmp = gsl_vector_calloc(
    seq->nterms);
  gsl_vector** exp_i_tmp = new gsl_vector*[seq->nseq];
  for (int i=0; i < seq->nseq; ++i) {
    exp_i_tmp[i] = gsl_vector_calloc(seq->nterms);
  }

  int n;

  gsl_vector* response = gsl_vector_calloc(seq->nterms);
  gsl_vector* exp_h_tmp = gsl_vector_calloc(seq->nterms);
  gsl_vector* d_tmp = gsl_vector_calloc(corpus->ndocs);
  gsl_vector* d_phi_var_tmp = gsl_vector_calloc(corpus->ndocs);

  //  assert(post->phi->size2 == documents_topics_t->size2);
  double* total_terms = (double*) malloc(sizeof(double) * corpus->ndocs);
  double* renormalization_totals = (double*) malloc(sizeof(double) * corpus->ndocs);
  for (k=0; k < documents_topics_t->size2; ++k) {
    // Set up W_phi and W_phi_var.
    for (int d=0; d < corpus->ndocs; ++d) {
      doc_t* doc = corpus->doc[d];
      total_terms[d] = 0.0;
      renormalization_totals[d] = 0.0;
      double log_norm_sum = 0.0;
      for (n=0; n < doc->nterms; ++n) {
	total_terms[d] += doc->count[n];
	if (FLAGS_normalize_docs == "normalize") {
	  renormalization_totals[d] = 1.0;
	} else if (FLAGS_normalize_docs == "log") {
	  renormalization_totals[d] += log(doc->count[n] + 1);
	} else if (FLAGS_normalize_docs == "log_norm") {
	  double weight = log(doc->count[n] / doc->total);
	  renormalization_totals[d] += weight;
	} else if (FLAGS_normalize_docs == "identity") {
	  renormalization_totals[d] += doc->count[n];
	} else if (FLAGS_normalize_docs == "occurrence") {
	  renormalization_totals[d] = 1.0;
	}
      }
      assert(doc->total == total_terms[d]);

      for (n=0; n < doc->nterms; ++n) {
	// Phi, for the doc's term n and topic k.
	double phi_d_n_k = gsl_matrix_get(phi[d], n, k);

	// This cast should happen automatically because total_terms is a double,
	// but we make the cast here to be explicit and to avoid bugs later.
	double number_terms;
	if (FLAGS_normalize_docs == "normalize") {
	  number_terms = ((double) doc->count[n]
			  / (double) total_terms[d]);
	} else if (FLAGS_normalize_docs == "log") {
	  number_terms = log(doc->count[n] + 1.0);
	} else if (FLAGS_normalize_docs == "log_norm") {
	  number_terms = log(doc->count[n] / total_terms[d]);
	  renormalization_totals[d] += log(doc->count[n] / total_terms[d]);
	} else if (FLAGS_normalize_docs == "identity") {
	  number_terms = doc->count[n];
	} else if (FLAGS_normalize_docs == "occurrence") {
	  number_terms = ((double) doc->count[n] / (double) total_terms[d]);
	  assert(doc->count[n] == 1);
	} else {
	  assert(0);
	}

	gsl_matrix_set(W_phi, doc->word[n], d,
		       number_terms * phi_d_n_k);
	gsl_matrix_set(W2_phi2, doc->word[n], d,
		       number_terms * number_terms
		       * phi_d_n_k * phi_d_n_k);
	gsl_matrix_set(W_phi_var, doc->word[n], d,
		       number_terms * number_terms
		       * (phi_d_n_k - phi_d_n_k * phi_d_n_k));
      }
    }

    gsl_vector_view document_weights = gsl_matrix_column(
      documents_topics_t,
      k);
    gsl_vector_view renormalized_document_weights = gsl_matrix_column(
      renormalized_documents_topics_t,
      k);
    assert(seq->topic[k]->e_log_prob->size2 == data->len);

    // Now, with w_phi_var, etc. set, determine
    // \sum_{i=t}^{T-1} r(...) h(t, i)
    SetExpHTmp(seq,
	       data,
	       t,
	       k,
	       n,
	       exp_h_tmp,
	       zw_tmp,
	       exp_i_tmp);

    // Next, set up the weighted response,
    // \exp_{-m + v / 2) \circ (m_{t+1} - m_t + v_t}).
    // Here we also subtract the current l's contribution to influence_sum_lgl.
    gsl_vector_view w_phi_l_t = gsl_matrix_column(seq->topic[k]->w_phi_l, t);
    gsl_vector_set_zero(response);
    for (int i = t; i < seq->nseq - 1; ++i) {
      gsl_vector_view total_influence_time_i =
	gsl_matrix_column(seq->influence_sum_lgl[k], i);
      gsl_vector_memcpy(zw_tmp, &w_phi_l_t.vector);
      gsl_vector_scale(zw_tmp, scaled_influence[i - t]);
      gsl_vector_sub(&total_influence_time_i.vector, zw_tmp);

      // Now, copy this total influence at time i back into zw_tmp.
      gsl_vector_memcpy(zw_tmp, &total_influence_time_i.vector);
      gsl_vector_mul(zw_tmp, exp_i_tmp[i]);

      gsl_vector_view mean_i_current =
	gsl_matrix_column(seq->topic[k]->e_log_prob, i);
      gsl_vector_view mean_i_next =
	gsl_matrix_column(seq->topic[k]->e_log_prob, i + 1);
      gsl_vector_view var_i_current =
	gsl_matrix_column(seq->topic[k]->variance, i + 1);
      gsl_vector_memcpy(terms_inc_tmp, &mean_i_next.vector);
      gsl_vector_sub(terms_inc_tmp, &mean_i_current.vector);
      gsl_vector_add(terms_inc_tmp, &var_i_current.vector);
      gsl_vector_sub(terms_inc_tmp, zw_tmp);
      assert(data->nterms == terms_inc_tmp->size);
      gsl_vector_mul(terms_inc_tmp, exp_i_tmp[i]);
      gsl_vector_scale(terms_inc_tmp, scaled_influence[i - t]);

      gsl_vector_add(response, terms_inc_tmp);
    }

    PrepareRegressionComponents(corpus,
				seq,
				k,
				W_phi,
				W_phi_var,
				d_phi_var_tmp,
				response,
				d_tmp,
				dd_tmp,
				&document_weights.vector,
				W_phi_tmp,
				exp_h_tmp);
    // Finally, solve for the document weights d!
    Solve(dd_tmp,
	  d_tmp,
	  &document_weights.vector);

    // Keep track of the iteration so we can dump certain stats
    // occasionally (but not always).
    static int dump_count = 0;
    ++dump_count;

    if (FLAGS_save_time == t) {
      outlog("Updating topic %ld, time %ld.", k, t);
      char name[400];
      sprintf(name, "%s%ld_%ld_weighted_document_terms.dat", root, k, t);
      FILE* f = fopen(name, "w");
      params_write_sparse_gsl_matrix(f, "W_phi", W_phi);
      fclose(f);

      sprintf(name, "%s%ld_%ld_weighted_document_terms_var.dat", root, k, t);
      f = fopen(name, "w");
      params_write_sparse_gsl_matrix(f, "W_phi_var", W_phi_var);
      fclose(f);

      sprintf(name, "%s%ld_%ld_phi.dat", root, k, t);
      f = fopen(name, "w");
      params_write_gsl_matrix(f, "phi 0", phi[0]);
      //     params_write_gsl_matrix(f, "phi 2", phi[1]);
      // params_write_gsl_matrix(f, "phi 2", phi[7]);
      fclose(f);

      sprintf(name, "%s%ld_%ld_weighted_document_terms_sq.dat", root, k, t);
      f = fopen(name, "w");
      params_write_sparse_gsl_matrix(f, "W2_phi2", W2_phi2);
      fclose(f);

      sprintf(name, "%s%ld_%ld_weighted_response.dat", root, k, t);
      f = fopen(name, "w");
      params_write_gsl_vector_multiline(f, "weighted_response", d_tmp);
      fclose(f);

      sprintf(name, "%s%ld_%ld_response.dat", root, k, t);
      f = fopen(name, "w");
      params_write_gsl_vector_multiline(f, "response", response);
      fclose(f);

      sprintf(name, "%s%ld_%ld_exp_h.dat", root, k, t);
      f = fopen(name, "w");
      params_write_gsl_vector_multiline(f, "exp_h", exp_h_tmp);
      fclose(f);

      if (dump_doc_stats || dump_count % 4 == 0) {
	sprintf(name, "%s%ld_%ld_document_document_matrix.dat", root, k, t);
	f = fopen(name, "w");
	params_write_gsl_matrix(f, "document_document_matrix", dd_tmp);
	fclose(f);

	sprintf(name, "%s%ld_%ld_exp_h.dat", root, k, t);
	f = fopen(name, "w");
	params_write_gsl_vector_multiline(f, "exp_h", exp_h_tmp);
	fclose(f);
      }
    }

    if (FLAGS_save_time == -1) {
      // First, dump phi's for the top topics.
      DumpTimeDocTopicStats(root, t, corpus, phi);
    }
    outlog("Done updating topic %ld, time %ld.", k, t);

    for (int d = 0; d < document_weights.vector.size; ++d) {
      gsl_vector_set(&renormalized_document_weights.vector, d,
		     vget(&document_weights.vector, d)
		     * renormalization_totals[d]);
    }

    // Now copy this and several products to
    // the sslm_var object.
    gsl_vector_view w_phi_l =
      gsl_matrix_column(seq->topic[k]->w_phi_l, t);
    gsl_blas_dgemv(CblasNoTrans,
		   1.0,
		   W_phi,
		   &document_weights.vector,
		   0.0,
		   &w_phi_l.vector);

    // Copy this value back into lgl.
    for (int i=t; i < seq->nseq - 1; ++i) {
      gsl_vector_view total_influence_time_i =
	gsl_matrix_column(seq->influence_sum_lgl[k], i);
      gsl_vector_memcpy(zw_tmp, &w_phi_l.vector);
      gsl_vector_scale(zw_tmp, scaled_influence[i - t]);
      gsl_vector_add(&total_influence_time_i.vector, zw_tmp);
    }

     // Keep track of the term we need to add to m_update_coeff.
     gsl_vector_memcpy(terms_inc_tmp, &w_phi_l.vector);
     gsl_vector_mul(terms_inc_tmp, &w_phi_l.vector);

     // Copy and square the document weights vector.
     for (int i = 0; i < xd_tmp->size; ++i) {
       double value = gsl_vector_get(&document_weights.vector, i);
       value = value * value + FLAGS_sigma_l * FLAGS_sigma_l;
       gsl_vector_set(xd_tmp, i, value);
     }
     gsl_blas_dgemv(CblasNoTrans,
		    1.0,
		    W_phi_var,
		    xd_tmp,
		    0.0,
		    yw_tmp);
     gsl_vector_add(terms_inc_tmp, yw_tmp);

     for (int i = 0; i < xd_tmp->size; ++i) {
       gsl_vector_set(xd_tmp, i, FLAGS_sigma_l * FLAGS_sigma_l);
     }
     gsl_blas_dgemv(CblasNoTrans,
		    1.0,
		    W2_phi2,
		    xd_tmp,
		    0.0,
		    yw_tmp);
     gsl_vector_add(terms_inc_tmp, yw_tmp);

     // Store an update coefficient for the beta updates.
     for (int i = t; i < seq->nseq; ++i) {
       gsl_vector_view m_update_coeff_h =
	 gsl_matrix_column(seq->topic[k]->m_update_coeff, i);
       if (t == 0) {
	 gsl_vector_set_zero(&m_update_coeff_h.vector);
       }
       gsl_vector_memcpy(yw_tmp, terms_inc_tmp);
       gsl_vector_scale(yw_tmp, scaled_influence[i - t]);
       gsl_vector_add(&m_update_coeff_h.vector, yw_tmp);
     }

     for (int i = t; i < seq->nseq; ++i) {
       gsl_vector_view m_update_coeff_g =
	 gsl_matrix_column(seq->topic[k]->m_update_coeff_g, i);
       if (t == 0) {
	 gsl_vector_set_zero(&m_update_coeff_g.vector);
       }
       gsl_vector_memcpy(yw_tmp, &w_phi_l.vector);
       gsl_vector_scale(yw_tmp, scaled_influence[i - t]);
       gsl_vector_add(&m_update_coeff_g.vector, yw_tmp);
     }

     for (int i = 0; i < corpus->ndocs; ++i) {
       double value = gsl_vector_get(&document_weights.vector, i);

       // While we're here, increment the likelihood.
       lhood += (-(value * value + FLAGS_sigma_l * FLAGS_sigma_l)
		 / (2.0 * FLAGS_sigma_d * FLAGS_sigma_d)
		 - 0.5 * log(2 * PI)
		 - log(FLAGS_sigma_d * FLAGS_sigma_d));
     }
  }
  free(total_terms);
  free(renormalization_totals);
  gsl_matrix_free(W_phi);
  gsl_matrix_free(W_phi_tmp);
  gsl_matrix_free(W2_phi2);
  gsl_matrix_free(W_phi_var);
  gsl_matrix_free(dd_tmp);
  gsl_vector_free(exp_h_tmp);
  gsl_vector_free(response);
  gsl_vector_free(terms_inc_tmp);
  for (int i=0; i < seq->nseq; ++i) {
    gsl_vector_free(exp_i_tmp[i]);
  }
  delete[] exp_i_tmp;
  gsl_vector_free(d_tmp);
  gsl_vector_free(d_phi_var_tmp);
  gsl_vector_free(xd_tmp);
  gsl_vector_free(yw_tmp);
  gsl_vector_free(zw_tmp);

  return lhood;
}

void make_lda_from_seq_slice(lda* lda_m,
			     lda_seq* lda_seq_m,
			     int time) {
     // set lda model topics
     // !!! note: we should be able to point to the view...

     int k;
     for (k = 0; k < lda_seq_m->ntopics; k++)
     {
	 // get topic
	 gsl_vector s =
	     gsl_matrix_column(lda_seq_m->topic[k]->e_log_prob,
			       time).vector;
	 gsl_vector d =
	     gsl_matrix_column(lda_m->topics, k).vector;
	 gsl_blas_dcopy(&s, &d);
     }
     gsl_blas_dcopy(lda_seq_m->alpha, lda_m->alpha);
 }

static gsl_matrix* g_alloc(lda_seq* model,
			   const corpus_seq_t* data,
			   int time) {
     gsl_matrix* g = gsl_matrix_calloc(model->nterms,
				       model->ntopics);
     double exp_m, m, m_next;
     for (int k = 0; k < model->ntopics; ++k) {
       for (int w=0; w < model->nterms; ++w) {
	 double variance_first = mget(model->topic[k]->variance, w, time);
	 double m = mget(model->topic[k]->e_log_prob, w, time);
	 double m_next;
	 exp_m = exp(-m + variance_first / 2.0);
	 gsl_matrix_set(g, w, k,
			(scaled_influence[0]
			 * -variance_first
			 * exp_m));

	 for (int i=time; i < model->nseq - 1; ++i) {
	   // This loop is kind of going overboard, but at least we
	   // do this once per E-M iteration.
	   double influence_other_times = 0.0;
	   for (int j = 0; j < i; ++j) {
	     exp_m = exp(-mget(model->topic[k]->e_log_prob, w, j)
			 + mget(model->topic[k]->variance, w, j) / 2.0);
	     // Note that we skip the other docs in this time period.
	     // Those get special treatment below.
	     if (j != time) {
	       influence_other_times += (
	          mget(model->topic[k]->w_phi_l, w, j)
		  * scaled_influence[i - j]
		  * exp_m);
	      }
	   }

	   m = mget(model->topic[k]->e_log_prob, w, i);
	   m_next = mget(model->topic[k]->e_log_prob, w, i + 1);
	   // Increment the current count by this value.
	   gsl_matrix_set(g, w, k,
			  mget(g, w, k) +
			  (scaled_influence[i - time]
			   * (m_next - m - influence_other_times)));
	 }
	 exp_m = exp(-m
		     + mget(model->topic[k]->variance, w, time) / 2.0);
	 gsl_matrix_set(g, w, k, mget(g, w, k) * exp_m);
       }
     }
     return g;
}

static gsl_matrix* g3_alloc(lda_seq* model,
			    const corpus_seq_t* data,
			    int time) {
     gsl_matrix* g = gsl_matrix_calloc(model->nterms,
				       model->ntopics);
     double exp_m, m, m_next, variance, total;
     for (int k = 0; k < model->ntopics; ++k) {
       for (int w=0; w < model->nterms; ++w) {
	 total = 0.0;
	 for (int i=time; i < model->nseq - 1; ++i) {
	   // This loop is kind of going overboard, but at least we
	   // do this once per E-M iteration.
	   variance = mget(model->topic[k]->variance, w, i + 1);
	   m = mget(model->topic[k]->e_log_prob, w, i);
	   m_next = mget(model->topic[k]->e_log_prob, w, i + 1);
	   exp_m = exp(-m + variance / 2.0);
	   total += (scaled_influence[i - time]
		     * exp_m
		     * (m_next - m + variance));
	 }
	 gsl_matrix_set(g, w, k, total);
       }
     }
     return g;
}

static gsl_matrix* g4_alloc(lda_seq* model,
		    const corpus_seq_t* data,
		    int time) {
     gsl_matrix* g = gsl_matrix_calloc(model->nterms,
				       model->ntopics);
     double exp_m, exp_m_scaled, m, total, variance, w_phi_l;
     for (int k = 0; k < model->ntopics; ++k) {
       for (int w=0; w < model->nterms; ++w) {
	 total = 0.0;
	 for (int i=time; i < model->nseq - 1; ++i) {
	   // This loop is kind of going overboard, but at least we
	   // do this once per E-M iteration.
	   variance = mget(model->topic[k]->variance, w, i + 1);
	   m = mget(model->topic[k]->e_log_prob, w, i);
	   exp_m = exp(-2.0 * m + 2.0 * variance);
	   exp_m_scaled = exp_m * scaled_influence[i - time];
	   for (int j=0; j <= i; ++j) {
	     w_phi_l = mget(model->topic[k]->w_phi_l, w, j);
	     total += exp_m_scaled * w_phi_l * scaled_influence[i - j];
	   }
	 }
	 gsl_matrix_set(g, w, k, total);
       }
     }
     return g;
}

static gsl_matrix* g5_alloc(lda_seq* model,
			    const corpus_seq_t* data,
			    int time) {
     gsl_matrix* g = gsl_matrix_calloc(model->nterms,
				       model->ntopics);
     double exp_m, m, total, variance;
     for (int k = 0; k < model->ntopics; ++k) {
       for (int w=0; w < model->nterms; ++w) {
	 total = 0.0;
	 for (int i=time; i < model->nseq - 1; ++i) {
	   // This loop is kind of going overboard, but at least we
	   // do this once per E-M iteration.
	   variance = mget(model->topic[k]->variance, w, i + 1);
	   m = mget(model->topic[k]->e_log_prob, w, i);
	   exp_m = exp(-2.0 * m + 2.0 * variance);
	   total += exp_m * (scaled_influence[i - time]
			     * scaled_influence[i - time]);
	 }
	 gsl_matrix_set(g, w, k, total);
       }
     }
     return g;
}

 /*
  * compute the likelihood of a sequential corpus under an LDA seq
  * model.  return the likelihood bound.
  *
  */

static void InferDTMSeq(const int K,
			unsigned int iter,
			unsigned int last_iter,
			const corpus_seq_t* data,
			gsl_matrix* gammas,
			gsl_matrix* lhoods,
			lda* lda_model,
			lda_post* post,
			lda_seq* model,
			gsl_matrix** suffstats,
			double* bound) {
  int doc_index = 0;
  for (int t = 0; t < data->len; t++) {
    // Prepare coefficients for the phi updates.  This change is
    // relatively painless.
    make_lda_from_seq_slice(lda_model, model, t);
    int ndocs = data->corpus[t]->ndocs;
    for (int d = 0; d < ndocs; d++) {
      gsl_vector gam   = gsl_matrix_row(gammas, doc_index).vector;
      gsl_vector lhood = gsl_matrix_row(lhoods, doc_index).vector;
      post->gamma = &gam;
      post->doc   = data->corpus[t]->doc[d];
      post->lhood = &lhood;
      double doc_lhood;
      // For now, only do the standard, phi-based update.
      if (iter == 0) {
	doc_lhood = fit_lda_post(d, t,
				 post, NULL,
				 NULL,
				 NULL,
				 NULL,
				 NULL);
      } else {
	doc_lhood = fit_lda_post(d, t, post, model, NULL,
				 NULL, NULL, NULL);
      }
      if (suffstats != NULL) {
	update_lda_seq_ss(t,
			  data->corpus[t]->doc[d],
			  post,
			  suffstats);
      }
      *bound += doc_lhood;
      doc_index++;
    }
  }
}

static void InferDIMSeq(const int K,
			unsigned int iter,
			unsigned int last_iter,
			const char* file_root,
			const corpus_seq_t* data,
			gsl_matrix* gammas,
			gsl_matrix* lhoods,
			lda* lda_model,
			lda_post* post,
			lda_seq* model,
			gsl_matrix** suffstats,
			double* bound) {
  int doc_index = 0;
  for (int t = 0; t < data->len; t++) {
    // Prepare coefficients for the phi updates.  This change is
    // relatively painless.
    gsl_matrix* g = g_alloc(model, data, t);

    gsl_matrix* g3_matrix = g3_alloc(model, data, t);
    gsl_matrix* g4_matrix = g4_alloc(model, data, t);
    gsl_matrix* g5_matrix = g5_alloc(model, data, t);

    make_lda_from_seq_slice(lda_model, model, t);
    int ndocs = data->corpus[t]->ndocs;
    gsl_matrix** phi_t = (gsl_matrix**) malloc(ndocs
					       * sizeof(gsl_matrix*));
    for (int d = 0; d < ndocs; d++) {
      gsl_vector gam   = gsl_matrix_row(gammas, doc_index).vector;
      gsl_vector lhood = gsl_matrix_row(lhoods, doc_index).vector;
      post->gamma = &gam;
      post->doc   = data->corpus[t]->doc[d];
      post->lhood = &lhood;
      double doc_lhood;
      // For now, only do the standard, phi-based update.
      if (iter == 0) {
	doc_lhood = fit_lda_post(d, t,
				 post, NULL,
				 NULL,
				 NULL,
				 NULL,
				 NULL);
      } else {
	doc_lhood = fit_lda_post(d, t, post, model, g,
				 g3_matrix, g4_matrix, g5_matrix);
      }
      if (suffstats != NULL) {
	update_lda_seq_ss(t,
			  data->corpus[t]->doc[d],
			  post,
			  suffstats);
      }
      phi_t[d] = gsl_matrix_alloc(post->doc->nterms, K);
      gsl_matrix_view phi_view = gsl_matrix_submatrix(
        post->phi,
	0, 0, post->doc->nterms, K);
      gsl_matrix_memcpy(phi_t[d], &phi_view.matrix);
      *bound += doc_lhood;
      doc_index++;
    }

    if (t < data->len - 1) {
      if (FLAGS_model == "fixed") {
	double l_bound = update_inf_var_fixed(
          model,
	  data,
	  // Also want a copy of phi for each doc.
	  // Can keep this as a vector for now.
	  phi_t,
	  t,
	  file_root,
	  last_iter || iter >= FLAGS_lda_sequence_min_iter);
	*bound += l_bound;
      }
    }

    for (int d=0; d < ndocs; ++d) {
      gsl_matrix_free(phi_t[d]);
    }
    free(phi_t);
    gsl_matrix_free(g);
    gsl_matrix_free(g3_matrix);
    gsl_matrix_free(g4_matrix);
    gsl_matrix_free(g5_matrix);
  }
}

double lda_seq_infer(lda_seq* model,
		     const corpus_seq_t* data,
		     gsl_matrix** suffstats,
		     gsl_matrix* gammas,
		     gsl_matrix* lhoods,
		     unsigned int iter,
		     unsigned int last_iter,
		     const char* file_root) {
  int K = model->ntopics; int W = model->nterms;
  double bound = 0.0;
  lda* lda_model = new_lda_model(K, W);
  lda_post post;
  post.phi = gsl_matrix_calloc(data->max_nterms, K);
  post.log_phi = gsl_matrix_calloc(data->max_nterms, K);
  post.model = lda_model;

  if (FLAGS_model == "fixed") {
    // First, pre-compute the functions f and g.
    InfluenceTotalFixed(model, data);
    InferDIMSeq(K,
		iter,
		last_iter,
		file_root,
		data,
		gammas,
		lhoods,
		lda_model,
		&post,
		model,
		suffstats,
		&bound);
  } else if (FLAGS_model == "dtm") {
    InferDTMSeq(K,
		iter,
		last_iter,
		data,
		gammas,
		lhoods,
		lda_model,
		&post,
		model,
		suffstats,
		&bound);
  } else {
    printf("Error.  Unknown model.\n");
    exit(1);
  }

  gsl_matrix_free(post.phi);
  gsl_matrix_free(post.log_phi);
  free_lda_model(lda_model);
  return(bound);
}

 /*
  * fit an lda sequence model:
  *
  * . for each time period
  * .     set up lda model with E[log p(w|z)] and \alpha
  * .     for each document
  * .         perform posterior inference
  * .         update sufficient statistics/likelihood
  * .
  * . maximize topics
  *
  */

double fit_lda_seq(lda_seq* m, const corpus_seq_t* data,
		   const corpus_seq_t* heldout, const char* file_root) {
     int K = m->ntopics, W = m->nterms;
     int k;

     // initialize sufficient statistics

     gsl_matrix* topic_suffstats[K];
     for (k = 0; k < K; k++) {
	 topic_suffstats[k] = gsl_matrix_calloc(W, data->len);
     }

     // set up variables

     char name[400];
     gsl_matrix* gammas = gsl_matrix_calloc(data->ndocs, K);
     gsl_matrix* lhoods = gsl_matrix_calloc(data->ndocs, K+1);
     gsl_matrix* heldout_gammas = NULL;
     gsl_matrix* heldout_lhoods = NULL;
     if (heldout != NULL)
     {
	 heldout_gammas = gsl_matrix_calloc(heldout->ndocs, K);
	 heldout_lhoods = gsl_matrix_calloc(heldout->ndocs, K+1);
     }

     double bound = 0, heldout_bound = 0, old_bound;
     double convergence = LDA_SEQ_EM_THRESH + 1;

     char root[400];
     sprintf(root, "%s/lda-seq/", file_root);
     make_directory(root);

     char em_log_filename[400];
     sprintf(em_log_filename, "%s/em_log.dat", file_root);
     FILE* em_log = fopen(em_log_filename, "w");

     // run EM

     int iter = 0;
     // LDA_INFERENCE_MAX_ITER = 1;
     short final_iters_flag = 0;
     unsigned int last_iter = 0;
     while (iter < FLAGS_lda_sequence_min_iter ||
	    ((final_iters_flag == 0 || convergence > LDA_SEQ_EM_THRESH)
	     && iter <= FLAGS_lda_sequence_max_iter)
	    && !last_iter) {

	 if (!(iter < FLAGS_lda_sequence_min_iter ||
	       ((final_iters_flag == 0 || convergence > LDA_SEQ_EM_THRESH)
		&& iter <= FLAGS_lda_sequence_max_iter))) {
	   last_iter = 1;
	 }
	 outlog("\nEM iter %3d\n", iter);
	 outlog("%s", "E step\n");
	 fprintf(em_log, "%17.14e %5.3e\n", bound, convergence);

	 old_bound = bound;
	 gsl_matrix_set_zero(gammas);
	 gsl_matrix_set_zero(lhoods);
	 if (heldout != NULL) {
	   gsl_matrix_set_zero(heldout_gammas);
	   gsl_matrix_set_zero(heldout_lhoods);
	 }

	 for (k = 0; k < K; k++) {
	     gsl_matrix_set_zero(topic_suffstats[k]);
	 }

	 // compute the likelihood of a sequential corpus under an LDA
	 // seq model and find the evidence lower bound.
	 bound = lda_seq_infer(m,
			       data,
			       topic_suffstats,
			       gammas,
			       lhoods,
			       iter,
			       last_iter,
			       file_root);
	 if (heldout != NULL) {
	   heldout_bound = lda_seq_infer(m,
					 heldout,
					 NULL,
					 heldout_gammas,
					 heldout_lhoods,
					 iter,
					 last_iter,
					 file_root);
	 }

	// print out the gammas and likelihoods.
        sprintf(name, "%s/gam.dat", root);
        mtx_fprintf(name, gammas);
        sprintf(name, "%s/lhoods.dat", root);
        mtx_fprintf(name, lhoods);
        if (heldout != NULL)
        {
            sprintf(name, "%s/heldout_lhoods.dat", root);
            mtx_fprintf(name, heldout_lhoods);
            sprintf(name, "%s/heldout_gam.dat", root);
            mtx_fprintf(name, heldout_gammas);
        }
        outlog("%s", "\nM step");

	// fit the variational distribution
        double topic_bnd = fit_lda_seq_topics(m, topic_suffstats);
        bound += topic_bnd;

        write_lda_seq(m, root);
        if ((bound - old_bound) < 0)
        {
            if (LDA_INFERENCE_MAX_ITER == 1)
                LDA_INFERENCE_MAX_ITER = 2;
            if (LDA_INFERENCE_MAX_ITER == 2)
                LDA_INFERENCE_MAX_ITER = 5;
            if (LDA_INFERENCE_MAX_ITER == 5)
                LDA_INFERENCE_MAX_ITER = 10;
            if (LDA_INFERENCE_MAX_ITER == 10)
                LDA_INFERENCE_MAX_ITER = 20;

            outlog( "\nWARNING: bound went down %18.14f; "
		    "increasing var iter to %d\n",
                    bound-old_bound, LDA_INFERENCE_MAX_ITER);
        }

	// check for convergence
        convergence = fabs((bound - old_bound) / old_bound);
        if (convergence < LDA_SEQ_EM_THRESH) {
            final_iters_flag = 1;
            LDA_INFERENCE_MAX_ITER = 500;
            outlog("starting final iterations : max iter = %d\n",
                   LDA_INFERENCE_MAX_ITER);
            convergence = 1.0;
        }
        outlog("\n(%02d) lda seq bound=% 15.7f; "
	       "heldout bound=% 15.7f, conv=% 15.7e\n",
               iter, bound, heldout_bound, convergence);
        iter++;
    }
    return(bound);
}

/*
 * read and write lda sequence variational distribution
 *
 */

void write_lda_seq(const lda_seq* model, const char* root) {
  char name[400];
  sprintf(name, "%sinfo.dat", root);
  FILE* f = fopen(name, "w");

  params_write_int(f, "NUM_TOPICS", model->ntopics);
  params_write_int(f, "NUM_TERMS", model->nterms);
  params_write_int(f, "SEQ_LENGTH", model->nseq);
  params_write_gsl_vector(f, "ALPHA", model->alpha);

  fclose(f);

  int k;
  for (k = 0; k < model->ntopics; k++) {
    const int tmp = k;
    outlog("\nwriting topic %03d", tmp);
    sprintf(name, "%stopic-%03d", root, tmp);
    write_sslm_var(model->topic[tmp], name);
  }

  if (FLAGS_model == "fixed") {
    for (int t=0; t < model->influence->ntime; ++t) {
      sprintf(name, "%sinfluence_time-%03d", root, t);
      outlog("\nwriting influence weights for time %d to %s", t, name);
      gsl_matrix* influence_t = model->influence->doc_weights[t];
      assert(model->ntopics == influence_t->size2);
      mtx_fprintf(name, influence_t);

      sprintf(name, "%srenormalized_influence_time-%03d", root, t);
      outlog("\nwriting influence weights for time %d to %s", t, name);
      influence_t = model->influence->renormalized_doc_weights[t];
      assert(model->ntopics == influence_t->size2);
      mtx_fprintf(name, influence_t);
    }
  }
}

// Read information about a particular model.
// This model should be named "{root}info.dat"
// and should contain the following rows:
// number_topics
// number_times
// alpha, as a gsl vector
lda_seq* read_lda_seq(const char* root, corpus_seq_t* data) {
  char name[400];
  lda_seq* model = (lda_seq*) malloc(sizeof(lda_seq));

  sprintf(name, "%sinfo.dat", root);
  FILE* f = fopen(name, "r");
  if (f == NULL) {
    outlog("Unable to open file %s.  Failing.", name);
    exit(1);
  }
  params_read_int(f, "NUM_TOPICS", &(model->ntopics));
  params_read_int(f, "NUM_TERMS", &(model->nterms));
  params_read_int(f, "SEQ_LENGTH", &(model->nseq));
  params_read_gsl_vector(f, "ALPHA", &(model->alpha));
  fclose(f);

  model->topic = (sslm_var**) malloc(sizeof(sslm_var*) * model->ntopics);

  for (int k = 0; k < model->ntopics; k++) {
    outlog( "reading topic %d", k);
    sprintf(name, "%stopic-%03d", root, k);
    model->topic[k] = read_sslm_var(name);

    model->topic[k]->w_phi_l =
      gsl_matrix_alloc(model->nterms, model->nseq);
    model->topic[k]->w_phi_sum =
      gsl_matrix_alloc(model->nterms, model->nseq);
    model->topic[k]->w_phi_l_sq =
      gsl_matrix_alloc(model->nterms, model->nseq);

    if (FLAGS_model == "dim"
	|| FLAGS_model == "regression") {
      sprintf(name, "%sw_phi_l-%d", root, k);
      mtx_fscanf(name, model->topic[k]->w_phi_l);

      sprintf(name, "%sw_phi_sum-%d", root, k);
      mtx_fscanf(name, model->topic[k]->w_phi_sum);

      sprintf(name, "%sw_phi_l_sq-%d", root, k);
      mtx_fscanf(name, model->topic[k]->w_phi_l_sq);
    }
  }

  if (FLAGS_model == "dim"
      || FLAGS_model == "regression"
      && data != NULL) {
    model->influence = (inf_var*) malloc(sizeof(inf_var));
    model->influence->doc_weights =
      (gsl_matrix**) malloc(sizeof(gsl_matrix*));
    int t;
    model->influence->ntime = model->nseq;
    for (t=0; t < model->nseq; ++t) {
      //	outlog("%d %d", t, model->influence->ntime);
      sprintf(name, "%sinfluence_time-%03d", root, t);
      outlog("\n reading influence weights for time %d from %s", t, name);
      model->influence->doc_weights[t] =
	gsl_matrix_alloc(data->corpus[t]->ndocs,
			 model->ntopics);
      mtx_fscanf(name, model->influence->doc_weights[t]);
    }
  } else {
    model->influence = NULL;
  }

  return(model);
}

/*
 * update lda sequence sufficient statistics from an lda posterior
 *
 */

void update_lda_seq_ss(int time,
                       const doc_t* doc,
                       const lda_post* post,
                       gsl_matrix** ss) {
    int K = post->phi->size2, N = doc->nterms;
    int k, n;

    for (k = 0; k < K; k++)
    {
        gsl_matrix* topic_ss = ss[k];
        for (n = 0; n < N; n++)
        {
            int w = doc->word[n];
            int c = doc->count[n];
            minc(topic_ss, w, time, c * mget(post->phi, n, k));
        }
    }
}

/*
 * fit lda sequence
 *
 */

double fit_lda_seq_topics(lda_seq* model,
                          gsl_matrix** ss) {
    double lhood = 0, lhood_term = 0;
    int k;

    for (k = 0; k < model->ntopics; k++)
    {
        outlog( "\nfitting topic %02d", k);
        lhood_term = fit_sslm(model->topic[k], ss[k]);
        lhood += lhood_term;
    }
    return(lhood);
}

/*
 * allocate lda seq
 *
 */

lda_seq* new_lda_seq(corpus_seq_t* data,
		     int W, int T, int K) {
    lda_seq* model = (lda_seq*) malloc(sizeof(lda_seq));

    model->ntopics = K;
    model->nterms = W;
    model->nseq = T;

    model->alpha = gsl_vector_alloc(K);
    model->topic = (sslm_var**) malloc(sizeof(sslm_var*) * K);

    // Create the vectors of total counts for each time.
    model->influence_sum_lgl = (gsl_matrix**) malloc(sizeof(gsl_matrix*) * K);

    for (int k = 0; k < K; k++) {
      //      model->w_phi_l = (gsl_matrix*) malloc(sizeof(gsl_matrix));
      // model->w_phi_l_sq = (gsl_matrix*) malloc(sizeof(gsl_matrix*));
      model->influence_sum_lgl[k] = gsl_matrix_calloc(W, T);

      model->topic[k] = sslm_var_alloc(W, T);
      if (k < FLAGS_fix_topics) {
	model->topic[k]->chain_variance = 1e-10;
      }
      model->topic[k]->w_phi_l = gsl_matrix_calloc(W, T);
      model->topic[k]->w_phi_sum = gsl_matrix_calloc(W, T);
      model->topic[k]->w_phi_l_sq = gsl_matrix_calloc(W, T);
    }
    model->influence = inf_var_alloc(K, data);

    return(model);
}

/*
 * initialize from sufficient statistics (expected counts).
 *
 */

void init_lda_seq_from_ss(lda_seq* model,
                          double topic_chain_variance,
                          double topic_obs_variance,
                          double alpha,
                          gsl_matrix* init_suffstats) {
    gsl_vector_set_all(model->alpha, alpha);

    for (int k = 0; k < model->ntopics; k++)
    {
        gsl_vector slice = gsl_matrix_column(init_suffstats, k).vector;
        sslm_counts_init(model->topic[k],
                         topic_obs_variance,
                         topic_chain_variance,
                         &slice);
	if (k < FLAGS_fix_topics) {
	  model->topic[k]->chain_variance = 1e-10;
	}
	model->topic[k]->w_phi_l = gsl_matrix_calloc(model->nterms,
						     model->nseq);
	model->topic[k]->w_phi_sum = gsl_matrix_calloc(model->nterms,
						       model->nseq);
	model->topic[k]->w_phi_l_sq = gsl_matrix_calloc(model->nterms,
							model->nseq);
    }

}

max_stars_repo_name: iwangjian/dtm-lab

max_issues_repo_name: iwangjian/topic-extractor

max_forks_repo_name: iwangjian/topic-extractor