Skip to content

Commit

Permalink
Remove SetTime in IsisInterfaceMapLineScan
Browse files Browse the repository at this point in the history 
It's easier to just use ISIS. However, I had to keep my own Ephemeris
LMA as ISIS is not reliable for projecting arbitrary 3D points back
into the LineScan camera. Luckily this is a largely unused section of
code.
  • Loading branch information
Zack Moratto committed May 22, 2011
1 parent 1303975 commit 655bc80
Show file tree
Hide file tree
Showing 2 changed files with 55 additions and 167 deletions.
182 changes: 54 additions & 128 deletions src/asp/IsisIO/IsisInterfaceMapLineScan.cc
View file
Expand Up @@ -6,8 +6,10 @@


// ASP
#include <asp/IsisIO/IsisInterfaceMapLineScan.h>
#include <vw/Cartography/SimplePointImageManipulation.h>
#include <vw/Camera/CameraModel.h>
#include <vw/Math/LevenbergMarquardt.h>
#include <asp/IsisIO/IsisInterfaceMapLineScan.h>

// ISIS
#include <ProjectionFactory.h>
Expand All @@ -18,85 +20,37 @@ using namespace asp::isis;

// Constructor
IsisInterfaceMapLineScan::IsisInterfaceMapLineScan( std::string const& filename ) :
IsisInterface( filename ) {
IsisInterface( filename ), m_projection( Isis::ProjectionFactory::CreateFromCube(m_label) ) {

// Gutting Isis::Camera
m_detectmap = m_camera->DetectorMap();
m_distortmap = m_camera->DistortionMap();
m_groundmap = m_camera->GroundMap();
m_focalmap = m_camera->FocalPlaneMap();
m_alphacube = new Isis::AlphaCube( m_label );
m_projection = Isis::ProjectionFactory::CreateFromCube( m_label );
m_camera->Radii( m_radii );
}

// Custom Function to help avoid over invoking the deeply buried
// functions of Isis::Sensor
void IsisInterfaceMapLineScan::SetTime( vw::Vector2 const& px,
bool calc ) const {
if ( px != m_c_location ) {
m_c_location = px;

m_projection->SetWorld( px[0],
px[1] );
Vector3 lon_lat_radius( m_projection->UniversalLongitude(),
m_projection->UniversalLatitude(), 0 );

// Solving for radius
if ( m_camera->HasElevationModel() ) {
lon_lat_radius[2] = m_camera->DemRadius( lon_lat_radius[1],
lon_lat_radius[0] );
} else {
Vector2 lon_lat = subvector(lon_lat_radius,0,2);
lon_lat = lon_lat * M_PI/180;
double bclon = m_radii[1]*cos(lon_lat[0]);
double aslon = m_radii[0]*sin(lon_lat[0]);
double cclat = m_radii[2]*cos(lon_lat[1]);
double xyradius = m_radii[0] * m_radii[1] / sqrt(bclon*bclon + aslon*aslon);
double xyslat = xyradius*sin(lon_lat[1]);
lon_lat_radius[2] = xyradius * m_radii[2] / sqrt(cclat*cclat + xyslat*xyslat );
}
lon_lat_radius[2] *= 1000;
Vector3 point = cartography::lon_lat_radius_to_xyz(lon_lat_radius);

// First seed LMA with an ephemeris time in the middle of the image
double middle = lines() / 2;
m_detectmap->SetParent( 1, m_alphacube->AlphaLine(middle) );
double start_e = m_camera->EphemerisTime();

// Build LMA
EphemerisLMA model( point, m_camera, m_distortmap, m_focalmap );
int status;
Vector<double> objective(1), start(1);
start[0] = start_e;
Vector<double> solution_e = math::levenberg_marquardt( model,
start,
objective,
status );
// Make sure we found ideal time
VW_ASSERT( status > 0,
MathErr() << " Unable to project point into linescan camera " );

// Setting to camera time to solution
m_camera->SetEphemerisTime( solution_e[0] );

if ( calc ) {
// Calculating Spacecraft position and pose
m_camera->InstrumentPosition(&m_center[0]);
m_center *= 1000;

std::vector<double> rot_inst = m_camera->InstrumentRotation()->Matrix();
std::vector<double> rot_body = m_camera->BodyRotation()->Matrix();
MatrixProxy<double,3,3> R_inst(&(rot_inst[0]));
MatrixProxy<double,3,3> R_body(&(rot_body[0]));
m_pose = Quat(R_body*transpose(R_inst));
}
}
}
// Custom Functions
class EphemerisLMA : public vw::math::LeastSquaresModelBase<EphemerisLMA> {
vw::Vector3 m_point;
Isis::Camera* m_camera;
Isis::CameraDistortionMap *m_distortmap;
Isis::CameraFocalPlaneMap *m_focalmap;
public:
typedef vw::Vector<double> result_type; // Back project result
typedef vw::Vector<double> domain_type; // Ephemeris time
typedef vw::Matrix<double> jacobian_type;

inline EphemerisLMA( vw::Vector3 const& point,
Isis::Camera* camera,
Isis::CameraDistortionMap* distortmap,
Isis::CameraFocalPlaneMap* focalmap ) : m_point(point), m_camera(camera), m_distortmap(distortmap), m_focalmap(focalmap) {}

inline result_type operator()( domain_type const& x ) const;
};


// LMA for projecting point to linescan camera
IsisInterfaceMapLineScan::EphemerisLMA::result_type
IsisInterfaceMapLineScan::EphemerisLMA::operator()( IsisInterfaceMapLineScan::EphemerisLMA::domain_type const& x ) const {
EphemerisLMA::result_type
EphemerisLMA::operator()( EphemerisLMA::domain_type const& x ) const {

// Setting Ephemeris Time
m_camera->SetEphemerisTime( x[0] );
Expand Down Expand Up @@ -144,24 +98,22 @@ IsisInterfaceMapLineScan::point_to_pixel( Vector3 const& point ) const {

// Make sure we found ideal time
VW_ASSERT( status > 0,
MathErr() << " Unable to project point into linescan camera " );
camera::PointToPixelErr() << " Unable to project point into linescan camera " );

// Setting to camera time to solution
m_camera->SetEphemerisTime( solution_e[0] );

// Working out pointing
m_camera->InstrumentPosition(&m_center[0]);
m_center *= 1000;
Vector3 look = normalize(point-m_center);
Vector3 center;
m_camera->InstrumentPosition(&center[0]);
Vector3 look = normalize(point-1000*center);

// Calculating Rotation to camera frame
std::vector<double> rot_inst = m_camera->InstrumentRotation()->Matrix();
std::vector<double> rot_body = m_camera->BodyRotation()->Matrix();
MatrixProxy<double,3,3> R_inst(&(rot_inst[0]));
MatrixProxy<double,3,3> R_body(&(rot_body[0]));
m_pose = Quat(R_body*transpose(R_inst));

look = inverse(m_pose).rotate( look );
look = transpose(R_body*transpose(R_inst))*look;
look = m_camera->FocalLength() * (look / look[2]);

// Projecting back on to ground to find out time
Expand All @@ -171,63 +123,37 @@ IsisInterfaceMapLineScan::point_to_pixel( Vector3 const& point ) const {

m_projection->SetGround( m_camera->UniversalLatitude(),
m_camera->UniversalLongitude() );
Vector2 result( m_projection->WorldX(),
m_projection->WorldY() );

// Caching result
m_c_location = result;

return result - Vector2(1,1);
return Vector2( m_projection->WorldX()-1,
m_projection->WorldY()-1 );
}

Vector3
IsisInterfaceMapLineScan::pixel_to_vector( Vector2 const& pix ) const {
Vector2 px = pix + Vector2(1,1);
SetTime( px, true );

m_projection->SetWorld(px[0],px[1]);
Vector3 lon_lat_radius( m_projection->UniversalLongitude(),
m_projection->UniversalLatitude(), 0 );

// Solving for radius
if ( m_camera->HasElevationModel() ) {
lon_lat_radius[2] = m_camera->DemRadius( lon_lat_radius[1],
lon_lat_radius[0] );
} else {
Vector2 lon_lat = subvector(lon_lat_radius,0,2);
lon_lat = lon_lat * M_PI/180;
double bclon = m_radii[1]*cos(lon_lat[0]);
double aslon = m_radii[0]*sin(lon_lat[0]);
double cclat = m_radii[2]*cos(lon_lat[1]);
double xyradius = m_radii[0] * m_radii[1] / sqrt(bclon*bclon + aslon*aslon);
double xyslat = xyradius*sin(lon_lat[1]);
lon_lat_radius[2] = xyradius * m_radii[2] / sqrt(cclat*cclat + xyslat*xyslat );
}
lon_lat_radius[2] *= 1000;
Vector3 point = cartography::lon_lat_radius_to_xyz(lon_lat_radius);
Vector3 result = normalize(point-m_center);

/*
m_camera->SetImage( px[0], px[1] );
double ip[3], cd[3];
m_camera->InstrumentPosition(ip);
m_camera->Coordinate(cd);
VectorProxy<double,3> ipv(ip), cdv(cd);
*/

return result;
IsisInterfaceMapLineScan::camera_center( Vector2 const& px ) const {
if (!m_projection->SetWorld( px[0]+1,
px[1]+1 ))
vw_throw( camera::PixelToRayErr() << "Failed to SetWorld." );
if (!m_groundmap->SetGround( m_projection->UniversalLatitude(),
m_projection->UniversalLongitude() ) )
vw_throw( camera::PixelToRayErr() << "Failed to SetGround." );
Vector3 position;
m_camera->InstrumentPosition( &position[0] );
return position * 1e3;
}

Vector3
IsisInterfaceMapLineScan::camera_center( Vector2 const& pix ) const {
Vector2 px = pix + Vector2(1,1);
SetTime( px, true );
return m_center;
IsisInterfaceMapLineScan::pixel_to_vector( Vector2 const& px ) const {
Vector3 sB = camera_center( px );
Vector3 p_pB;
m_camera->Sensor::Coordinate( &p_pB[0] );
return normalize(p_pB*1000 - sB);
}

Quat
IsisInterfaceMapLineScan::camera_pose( Vector2 const& pix ) const {
Vector2 px = pix + Vector2(1,1);
SetTime( px, true );
return m_pose;
IsisInterfaceMapLineScan::camera_pose( Vector2 const& px ) const {
camera_center( px );
std::vector<double> rot_inst = m_camera->InstrumentRotation()->Matrix();
std::vector<double> rot_body = m_camera->BodyRotation()->Matrix();
MatrixProxy<double,3,3> R_inst(&(rot_inst[0]));
MatrixProxy<double,3,3> R_body(&(rot_body[0]));
return Quat(R_body*transpose(R_inst));
}
40 changes: 1 addition & 39 deletions src/asp/IsisIO/IsisInterfaceMapLineScan.h
View file
Expand Up @@ -14,15 +14,12 @@

// ASP & VW
#include <asp/IsisIO/IsisInterface.h>
#include <vw/Math/LevenbergMarquardt.h>

// Isis
#include <Projection.h>
#include <CameraDetectorMap.h>
#include <CameraDistortionMap.h>
#include <CameraGroundMap.h>
#include <CameraFocalPlaneMap.h>
#include <AlphaCube.h>

namespace asp {
namespace isis {
Expand All @@ -32,13 +29,6 @@ namespace isis {
public:
IsisInterfaceMapLineScan( std::string const& file );

virtual ~IsisInterfaceMapLineScan() {
if ( m_projection )
delete m_projection;
if ( m_alphacube )
delete m_alphacube;
}

virtual std::string type() { return "MapLineScan"; }

// Standard Methods
Expand All @@ -56,39 +46,11 @@ namespace isis {
protected:

// Custom Variables
Isis::Projection *m_projection;
Isis::CameraDetectorMap *m_detectmap;
boost::scoped_ptr<Isis::Projection> m_projection;
Isis::CameraDistortionMap *m_distortmap;
Isis::CameraGroundMap *m_groundmap;
Isis::CameraFocalPlaneMap *m_focalmap;
Isis::AlphaCube *m_alphacube;

private:

// Custom Functions
mutable vw::Vector2 m_c_location;
mutable vw::Vector3 m_center;
mutable vw::Quat m_pose;
double m_radii[3];
void SetTime( vw::Vector2 const& px,
bool calc=false ) const;
class EphemerisLMA : public vw::math::LeastSquaresModelBase<EphemerisLMA> {
vw::Vector3 m_point;
Isis::Camera* m_camera;
Isis::CameraDistortionMap *m_distortmap;
Isis::CameraFocalPlaneMap *m_focalmap;
public:
typedef vw::Vector<double> result_type; // Back project result
typedef vw::Vector<double> domain_type; // Ephemeris time
typedef vw::Matrix<double> jacobian_type;

inline EphemerisLMA( vw::Vector3 const& point,
Isis::Camera* camera,
Isis::CameraDistortionMap* distortmap,
Isis::CameraFocalPlaneMap* focalmap ) : m_point(point), m_camera(camera), m_distortmap(distortmap), m_focalmap(focalmap) {}

inline result_type operator()( domain_type const& x ) const;
};
};

}}
Expand Down

0 comments on commit 655bc80

Please sign in to comment.