rpl/rgrl/rgrl_trans_similarity.cxx

#include "rgrl_trans_similarity.h"
//:
// \file
// \author Amitha Perera
// \date   Feb 2003

#include <cassert>
#include <utility>

#ifdef _MSC_VER
#  include "vcl_msvc_warnings.h"
#endif
#include "vnl/vnl_fastops.h"
#include <vnl/algo/vnl_svd.h>
#include <rgrl/rgrl_util.h>

rgrl_trans_similarity::
rgrl_trans_similarity( unsigned int dimension )
  : A_( vnl_matrix<double>( dimension, dimension, vnl_matrix_identity ) ),
    trans_( vnl_vector<double>( dimension, 0.0 ) ),
    from_centre_( dimension, 0.0 )
{
}

rgrl_trans_similarity::rgrl_trans_similarity(vnl_matrix<double> rot_and_scale,
                                             vnl_vector<double> const &in_trans)
    : A_(std::move(rot_and_scale)), trans_(in_trans),
      from_centre_(in_trans.size(), 0.0) {
  assert ( A_.rows() == A_.cols() );
  assert ( A_.rows() == trans_.size() );
  // assert ( ( A_.rows() != 2 || covar_.rows() == 4 ) ); // 2d has 4 params
  // assert ( ( A_.rows() != 3 || covar_.rows() == 7 ) ); // 3d has 7 params
}

rgrl_trans_similarity::rgrl_trans_similarity(vnl_matrix<double> rot_and_scale,
                                             vnl_vector<double> const &in_trans,
                                             vnl_matrix<double> const &in_covar)
    : rgrl_transformation(in_covar), A_(std::move(rot_and_scale)),
      trans_(in_trans), from_centre_(in_trans.size(), 0.0) {
  assert ( A_.rows() == A_.cols() );
  assert ( A_.rows() == trans_.size() );
  if ( is_covar_set() ) {
    assert ( covar_.rows() == covar_.cols() );
    assert ( ( A_.rows() != 2 || covar_.rows() == 4 ) ); // 2d has 4 params
    assert ( ( A_.rows() != 3 || covar_.rows() == 7 ) ); // 3d has 7 params
  }
}

rgrl_trans_similarity::rgrl_trans_similarity(
    vnl_matrix<double> in_A, vnl_vector<double> const &in_trans,
    vnl_matrix<double> const &in_covar, vnl_vector<double> in_from_centre,
    vnl_vector<double> const &in_to_centre)
    : rgrl_transformation(in_covar), A_(std::move(in_A)),
      trans_(in_trans + in_to_centre), from_centre_(std::move(in_from_centre)) {
  assert ( A_.rows() == A_.cols() );
  assert ( A_.rows() == trans_.size() );
  assert ( from_centre_.size() == trans_.size() );
  if ( is_covar_set() ) {
    assert ( covar_.rows() == covar_.cols() );
    assert ( ( A_.rows() != 2 || covar_.rows() == 4 ) ); // 2d has 4 params
    assert ( ( A_.rows() != 3 || covar_.rows() == 7 ) ); // 3d has 7 params
  }
}

void
rgrl_trans_similarity::
map_loc( vnl_vector<double> const& from,
         vnl_vector<double>      & to   ) const
{
  assert ( from.size() == A_.rows() );
  // for efficiency, rewrite the following as:
  // to = A_ * (from-from_centre_) + trans_;
  //
  vnl_fastops::Ab( to, A_, from-from_centre_ );
  to += trans_;
}

void rgrl_trans_similarity::map_dir(vnl_vector<double> const & from_loc,
                                    vnl_vector<double> const &from_dir,
                                    vnl_vector<double> &to_dir) const {
  assert ( from_loc.size() == A_.cols() );
  assert ( from_dir.size() == A_.cols() );
  to_dir = A_ * from_dir;
  to_dir.normalize();
}

vnl_matrix<double>
rgrl_trans_similarity::
transfer_error_covar( vnl_vector<double> const& p  ) const
{
  assert ( is_covar_set() );
  assert ( p.size() == 2);  // only deal with 2D for now
  assert ( A_.rows() == 2); // only deal with 2D for now

  vnl_matrix<double> temp( 2, 4, 0.0 );
  temp(0,0) = p[0]-from_centre_[0];
  temp(0,1) = from_centre_[1] - p[1];
  temp(0,2) = 1;
  temp(1,0) = p[1]-from_centre_[1];
  temp(1,1) = p[0] - from_centre_[0];
  temp(1,3) = 1;

  return temp * covar_ * temp.transpose();
}

vnl_matrix<double> const&
rgrl_trans_similarity::
A() const
{
  return A_;
}

double
rgrl_trans_similarity::
scaling() const
{
  assert( A_.rows() && A_.cols() );
  vnl_vector<double> tmp = A_.get_column(0);
  return tmp.magnitude();
}

vnl_vector<double>
rgrl_trans_similarity::
t() const
{
  return trans_ - A_ * from_centre_;
}

void
rgrl_trans_similarity::
inv_map( const vnl_vector<double>& to,
         bool initialize_next,
         const vnl_vector<double>& to_delta,
         vnl_vector<double>& from,
         vnl_vector<double>& from_next_est) const
{
  constexpr double epsilon = 0.01;
  vnl_vector<double> to_est = this->map_location(from);

  // compute the inverse of the Jacobian, which is the A_^-1
  vnl_svd<double> svd( A_ );
  vnl_matrix<double> J_inv = svd.inverse();

  // update from to become true inv_map of to, based on A^-1 and (to - to_est)
  if (vnl_vector_ssd(to, to_est) > epsilon*epsilon) {
    from += J_inv * (to - to_est);
  }
  if ( initialize_next ) {
    from_next_est = from + (J_inv * to_delta);
  }
}

void
rgrl_trans_similarity::
inv_map( const vnl_vector<double>& to,
         vnl_vector<double>& from ) const
{
  vnl_svd<double> svd( A_ );
  from = svd.inverse()*to - svd.inverse()*trans_ + from_centre_;
}

rgrl_transformation_sptr
rgrl_trans_similarity::
inverse_transform( ) const
{
  vnl_svd<double> svd( A() );
  vnl_matrix<double> invA = svd.inverse();
  rgrl_transformation_sptr result =  new rgrl_trans_similarity( invA, -invA * t() );

  const unsigned m = scaling_factors_.size();
  if ( m > 0 ) {
    vnl_vector<double> scaling( m );
    for ( unsigned int i=0; i<m; ++i )
      scaling[i] = 1.0 / scaling_factors_[i];
    result->set_scaling_factors( scaling );
  }

  return result;
}


void
rgrl_trans_similarity::
jacobian_wrt_loc( vnl_matrix<double>& jac, vnl_vector<double> const& /*from_loc*/ ) const
{
  jac = A_;
}

rgrl_transformation_sptr
rgrl_trans_similarity::
scale_by( double scale ) const
{
  rgrl_transformation_sptr xform
    = new rgrl_trans_similarity( A_, trans_ * scale,
                                 covar_, from_centre_ * scale,
                                 vnl_vector<double>(from_centre_.size(), 0.0) );
  xform->set_scaling_factors( this->scaling_factors() );
  return xform;
}

void
rgrl_trans_similarity::
write( std::ostream& os ) const
{
  // tag
  os << "SIMILARITY\n"
  // parameters
     << t().size() << std::endl
     << A_ << trans_ << ' ' << from_centre_ << std::endl;

  // parent
  rgrl_transformation::write( os );
}

bool
rgrl_trans_similarity::
read( std::istream& is )
{
  int dim;

  // skip empty lines
  rgrl_util_skip_empty_lines( is );

  std::string str;
  std::getline( is, str );

  // The token should appear at the beginning of line
  if ( str.find( "SIMILARITY" ) != 0 ) {
    WarningMacro( "The tag is not SIMILARITY. reading is aborted.\n" );
    return false;
  }

  // input global xform
  dim=-1;
  is >> dim;
  if ( dim > 0 ) {
    A_.set_size( dim, dim );
    trans_.set_size( dim );
    from_centre_.set_size( dim );
    is >> A_ >> trans_ >> from_centre_;
  }

  // parent
  return is.good() && rgrl_transformation::read( is );
}

//: make a clone copy
rgrl_transformation_sptr
rgrl_trans_similarity::
clone() const
{
  return new rgrl_trans_similarity( *this );
}