diff --git a/Makefile b/Makefile index e85f338ed23..bc33985be18 100644 --- a/Makefile +++ b/Makefile @@ -169,9 +169,12 @@ LIBRARIES += pthread \ glog gflags protobuf leveldb snappy \ lmdb \ boost_system \ + python2.7 \ + boost_python \ + boost_numpy \ hdf5_hl hdf5 \ opencv_core opencv_highgui opencv_imgproc -PYTHON_LIBRARIES := boost_python python2.7 +PYTHON_LIBRARIES := boost_python boost_numpy python2.7 opencv_core opencv_highgui opencv_imgproc WARNINGS := -Wall -Wno-sign-compare ############################## diff --git a/include/caffe/data_layers.hpp b/include/caffe/data_layers.hpp index 7ccb7a6e805..3ff4654ac1f 100644 --- a/include/caffe/data_layers.hpp +++ b/include/caffe/data_layers.hpp @@ -20,7 +20,6 @@ #include #include #include - namespace caffe { #define HDF5_DATA_DATASET_NAME "data" @@ -302,7 +301,7 @@ class VideoDataLayer : public BasePrefetchingDataLayer { virtual void InternalThreadEntry(); bool ReadVideoFrameToDatum(const string& filename, size_t id, size_t persp, const int height, const int width, Datum* datum); - + void setPerspective(); //inline bool ReadVideoBatchToDatum(const string& filename, std::vector frameIds, // std::vectortrans, Datum* datum) { // return ReadVideoBatchToDatum(filename, frameIds, trans, 0, 0, datum); @@ -310,9 +309,41 @@ class VideoDataLayer : public BasePrefetchingDataLayer { vector, std::vector > > > lines_; int lines_id_; + std::vector mTransforms; cv::VideoCapture* cap; }; + +/** + * @brief Provides multilane labels to the Net. + * + * TODO(dox): thorough documentation for Forward and proto params. + */ +template +class MultilaneLabelLayer : public BasePrefetchingDataLayer { + public: + explicit MultilaneLabelLayer(const LayerParameter& param) + : BasePrefetchingDataLayer(param) {} + virtual ~MultilaneLabelLayer(); + virtual void DataLayerSetUp(const vector*>& bottom, + vector*>* top); + + virtual inline LayerParameter_LayerType type() const { + return LayerParameter_LayerType_VIDEO_DATA; + } + virtual inline int ExactNumBottomBlobs() const { return 0; } + virtual inline int ExactNumTopBlobs() const { return 1; } + + protected: + shared_ptr prefetch_rng_; + virtual void ShuffleBatches(); + virtual void InternalThreadEntry(); + //boost::numpy::ndarray ReadLabelBatch(const string& filename, std::vector &frame_ids, std::vector trans, const int height, const int width); + + vector, std::vector > > > lines_; + int lines_id_; +}; + /** * @brief Provides data to the Net from memory. * diff --git a/models/brody/train_val_brody.prototxt b/models/brody/train_val_brody.prototxt index b70ea376e0e..6f45e3b47d0 100644 --- a/models/brody/train_val_brody.prototxt +++ b/models/brody/train_val_brody.prototxt @@ -18,11 +18,10 @@ layers { # Bounding box label and pixel label. layers { name: "label" - type: DATA + type: MULTILANE_LABEL top: "label" - data_param { - source: "/deep/group/driving_data/twangcat/lmdb/driving_label_train" - backend: LMDB + multilane_label_param { + source: "/scail/group/deeplearning/driving_data/twangcat/schedules/q50_multilane_planar_train_schedule1_batch20_2cam.txt" batch_size: 20 } include: { phase: TRAIN } diff --git a/prep_opencv.sh b/prep_opencv.sh new file mode 100755 index 00000000000..1d3b113b340 --- /dev/null +++ b/prep_opencv.sh @@ -0,0 +1,6 @@ +#!/bin/bash +export PKG_CONFIG_PATH=/deep/group/driving_data/twangcat/env_deep6/opencv/lib/pkgconfig/ +export LD_LIBRARY_PATH=/deep/group/driving_data/twangcat/env_deep6/opencv/lib/:$LD_LIBRARY_PATH +export PATH=/deep/group/driving_data/twangcat/env_deep6/opencv/include/:$PATH +export CPLUS_INCLUDE_PATH=/deep/group/driving_data/twangcat/env_deep6/opencv/include/:$CPLUS_INCLUDE_PATH + diff --git a/src/caffe/layer_factory.cpp b/src/caffe/layer_factory.cpp index 6002bd07f0e..a20f2c6e636 100644 --- a/src/caffe/layer_factory.cpp +++ b/src/caffe/layer_factory.cpp @@ -222,6 +222,8 @@ Layer* GetLayer(const LayerParameter& param) { return new LRNLayer(param); case LayerParameter_LayerType_MEMORY_DATA: return new MemoryDataLayer(param); + case LayerParameter_LayerType_MULTILANE_LABEL: + return new MultilaneLabelLayer(param); case LayerParameter_LayerType_MVN: return new MVNLayer(param); case LayerParameter_LayerType_MULTINOMIAL_LOGISTIC_LOSS: diff --git a/src/caffe/layers/multilane_label_layer.cpp b/src/caffe/layers/multilane_label_layer.cpp new file mode 100644 index 00000000000..27257e2b26c --- /dev/null +++ b/src/caffe/layers/multilane_label_layer.cpp @@ -0,0 +1,213 @@ +#include // NOLINT(readability/streams) +#include // NOLINT(readability/streams) +#include +#include +#include +#include +#include "caffe/data_layers.hpp" +#include "caffe/layer.hpp" +#include "caffe/util/io.hpp" +#include "caffe/util/math_functions.hpp" +#include "caffe/util/rng.hpp" +#include +#include +#include +#include + + +namespace py = boost::python; // create namespace variable for boost::python +namespace np = boost::numpy; // create namespace variable for boost::python +namespace caffe { + +//np::ndarray ReadLabelBatch(const string& filename, std::vector &frame_ids, std::vector &trans, +// const int height, const int width) { +//}; +template +MultilaneLabelLayer::~MultilaneLabelLayer() { + this->JoinPrefetchThread(); +} + + +template +void MultilaneLabelLayer::DataLayerSetUp(const vector*>& bottom, + vector*>* top) { + const int new_height = this->layer_param_.multilane_label_param().new_height(); + const int new_width = this->layer_param_.multilane_label_param().new_width(); + CHECK((new_height == 0 && new_width == 0) || + (new_height > 0 && new_width > 0)) << "Current implementation requires " + "new_height and new_width to be set at the same time."; + // Read the file with filenames and labels + const string& source = this->layer_param_.multilane_label_param().source(); + LOG(INFO) << "Opening schedule file " << source; + std::ifstream infile(source.c_str()); + + + + string batch_string; + + string filename; + //while (infile >> batch_string) { + while (getline (infile, batch_string)) { + if(!infile) + { + if(infile.eof()) + { + LOG(INFO) << "Reached EOF of schedule file."; + break; + } + else + LOG(FATAL)<< "Error while reading schedule file. Possibly corrupted."; + } + std::vector batch_fields; + // first split a line into fields with delimiter ",". Fields should be [filename, frame_ids, transform_ids] + boost::split(batch_fields, batch_string, boost::is_any_of(","), + boost::token_compress_on); + if(batch_fields.size()!=3) + LOG(FATAL) << "Each line must have 3 fields separated by comma, " + < frame_ids_str; + std::vector frame_ids; + boost::split(frame_ids_str, batch_fields[1], boost::is_any_of(" "), + boost::token_compress_on); + for (int f=0; f trans_ids_str; + std::vector trans_ids; + boost::split(trans_ids_str, batch_fields[2], boost::is_any_of(" "), + boost::token_compress_on); + for (int f=0; flayer_param_.multilane_label_param().shuffle()) { + // randomly shuffle data + LOG(INFO) << "Shuffling batches"; + const unsigned int prefetch_rng_seed = caffe_rng_rand(); + prefetch_rng_.reset(new Caffe::RNG(prefetch_rng_seed)); + ShuffleBatches(); + } + LOG(INFO) << "A total of " << lines_.size() << " batches."; + + lines_id_ = 0; + // Check if we would need to randomly skip a few data points + if (this->layer_param_.multilane_label_param().rand_skip()) { + unsigned int skip = caffe_rng_rand() % + this->layer_param_.multilane_label_param().rand_skip(); + LOG(INFO) << "Skipping first " << skip << " data points."; + CHECK_GT(lines_.size(), skip) << "Not enough points to skip"; + lines_id_ = skip; + } + const int crop_size = this->layer_param_.transform_param().crop_size(); + const int batch_size = this->layer_param_.multilane_label_param().batch_size(); + if (crop_size > 0) { + (*top)[0]->Reshape(batch_size, 80, crop_size, crop_size); + this->prefetch_data_.Reshape(batch_size, 80, crop_size, + crop_size); + } else { + (*top)[0]->Reshape(batch_size, 80, 15,20); + this->prefetch_data_.Reshape(batch_size, 80, 15,20); + } + LOG(INFO) << "output data size: " << (*top)[0]->num() << "," + << (*top)[0]->channels() << "," << (*top)[0]->height() << "," + << (*top)[0]->width(); + // label + //(*top)[1]->Reshape(batch_size, 1, 1, 1); + this->prefetch_label_.Reshape(batch_size, 1, 1, 1); + // datum size + this->datum_channels_ = 80; + this->datum_height_ = 15; + this->datum_width_ = 20; + this->datum_size_ = 80*15*20; +} + +template +void MultilaneLabelLayer::ShuffleBatches() { + caffe::rng_t* prefetch_rng = + static_cast(prefetch_rng_->generator()); + shuffle(lines_.begin(), lines_.end(), prefetch_rng); +} + + + +// This function is used to create a thread that prefetches the data. +template +void MultilaneLabelLayer::InternalThreadEntry() { + CHECK(this->prefetch_data_.count()); + Dtype* top_data = this->prefetch_data_.mutable_cpu_data(); + //Dtype* top_label = this->prefetch_label_.mutable_cpu_data(); + MultilaneLabelParameter multilane_label_param = this->layer_param_.multilane_label_param(); + const int batch_size = multilane_label_param.batch_size(); + const int new_height = multilane_label_param.new_height(); + const int new_width = multilane_label_param.new_width(); + + // datum scales + const int lines_size = lines_.size(); + string filename = lines_[lines_id_].first; + std::vector frameIds = lines_[lines_id_].second.first; + std::vector trans = lines_[lines_id_].second.second; + if (batch_size!=frameIds.size() || batch_size!=trans.size()) + LOG(ERROR)<<"Frame count mismatch!"; + LOG(INFO)<<"reading label batch "<(); + py::tuple shape = py::make_tuple(batch_size) ; + py::tuple stride = py::make_tuple(4) ; + py::object own1; + py::object own2; + int* framePtr = const_cast(&frameIds[0]); + int* transPtr = const_cast(&trans[0]); + np::ndarray frameArr = np::from_data(framePtr,dt,shape,stride,own1); + np::ndarray transArr = np::from_data(transPtr,dt,shape,stride,own2); + // call function + arr_handle = reader.attr("runLabelling")(filename, frameArr, transArr); + }catch(py::error_already_set const &){ + PyErr_Print(); + LOG(FATAL) <<"numpy label reading function failed!"; + } + np::ndarray pArray = np::from_object(arr_handle); + + int array_size = py::extract(pArray.attr("size")); + LOG(INFO)<<"numpy array size = "<= lines_size) { + // We have reached the end. Restart from the first. + DLOG(INFO) << "Restarting data prefetching from start."; + lines_id_ = 0; + if (this->layer_param_.image_data_param().shuffle()) { + ShuffleBatches(); + } + } +} + +INSTANTIATE_CLASS(MultilaneLabelLayer); + +} // namespace caffe diff --git a/src/caffe/layers/video_data_layer.cpp b/src/caffe/layers/video_data_layer.cpp index ac814fbbe0f..1563cbebdfd 100644 --- a/src/caffe/layers/video_data_layer.cpp +++ b/src/caffe/layers/video_data_layer.cpp @@ -10,6 +10,10 @@ #include "caffe/util/math_functions.hpp" #include "caffe/util/rng.hpp" #include +#include +#include +namespace py = boost::python; // create namespace variable for boost::python +namespace np = boost::numpy; // create namespace variable for boost::python namespace caffe { template @@ -41,8 +45,8 @@ bool VideoDataLayer:: ReadVideoFrameToDatum(const string& filename, size_ cv_img = cv_img_origin; } // apply perspective transform - //cv::Mat warpMatrix = mTransforms[persp+(cam_num-1)*numPersp]; - //cv::warpPerspective(cv_img, cv_img, warpMatrix, frame.size(), cv::INTER_LINEAR, cv::BORDER_REPLICATE); + cv::Mat warpMatrix = this->mTransforms[persp]; + cv::warpPerspective(cv_img, cv_img, warpMatrix, cv_img.size(), cv::INTER_LINEAR, cv::BORDER_REPLICATE); // copy data to datum int num_channels = 3; datum->set_channels(num_channels); @@ -63,6 +67,59 @@ bool VideoDataLayer:: ReadVideoFrameToDatum(const string& filename, size_ return true; } + +template +void VideoDataLayer::setPerspective() { + + /*try{ + Py_Initialize(); + PyRun_SimpleString("import sys"); + PyRun_SimpleString("sys.path.append(\"/afs/cs.stanford.edu/u/twangcat/scratch/caffenet/src/caffe/py_lane_label_reader/\")"); + np::initialize(); + }catch(py::error_already_set const &){ + PyErr_Print(); + LOG(FATAL) <<"boost python and numpy initialization failed."; + } + py::object list; + try{ + // initialize python helper class + py::object module = py::import("perspective_reader"); + py::object readerClass = module.attr("PerspectiveReader"); + py::object reader = readerClass(); + list = reader.attr("read")(); + }catch(py::error_already_set const &){ + PyErr_Print(); + LOG(FATAL) <<"numpy perspective reading function failed!"; + } + int num_persps = py::len(list); + for(int i=0;imTransforms.push_back(cv::Mat::eye(3, 3, CV_32F, persp_data)); + } +*/ + float persp_data[] = {2.00000000e+00, 4.85722573e-16, -1.98951966e-13, 1.55172414e-01, 2.20689655e+00, -9.93103448e+01, 2.35813973e-18, 1.46367293e-18, 2.00000000e+00}; + // essential to do a clone here otherwise data is not copied properly. not sure why + this->mTransforms.push_back(cv::Mat(3, 3, CV_32F, persp_data).clone()); + persp_data = {2.00000000e+00, -2.60991180e-16, -1.35814320e-13, -1.55172414e-01, 2.20689655e+00, -7.89095352e-14, -1.39242621e-19, -7.96792478e-19, 2.00000000e+00}; + this->mTransforms.push_back(cv::Mat(3, 3, CV_32F, persp_data).clone()); + persp_data = {2.22608696e+00, 3.01449275e-01, -1.44695652e+02, -1.74860126e-15, 2.00000000e+00, -9.94759830e-14, 2.00577402e-18, -4.60785923e-18, 2.00000000e+00}; + this->mTransforms.push_back(cv::Mat(3, 3, CV_32F, persp_data).clone()); + persp_data = {2.22608696e+00, -2.08695652e-01, -4.81606942e-14, -1.21601236e-15, 2.06956522e+00, -2.68140544e-14, 1.30767163e-18, 1.44927536e-04, 2.00000000e+00}; + this->mTransforms.push_back(cv::Mat(3, 3, CV_32F, persp_data).clone()); + persp_data = {1.76207983e+00, 2.42673669e-16, -1.35389096e-13, -1.65464295e-01, 2.18780567e+00, -8.38443898e-14, -6.37686030e-04, 3.91261819e-04, 2.00000000e+00}; + this->mTransforms.push_back(cv::Mat(3, 3, CV_32F, persp_data).clone()); + persp_data = {2.15126050e+00, -2.48222366e-01, 1.11700065e+01, -7.49400542e-16, 2.46153846e+00, -1.10769231e+02, 2.33103467e-18, 8.13151629e-19, 2.00000000e+00}; + this->mTransforms.push_back(cv::Mat(3, 3, CV_32F, persp_data).clone()); + persp_data = {1., 0., 0., 0., 1., 0., 0., 0., 1.}; + this->mTransforms.push_back(cv::Mat(3, 3, CV_32F, persp_data).clone()); +} + + + template void VideoDataLayer::DataLayerSetUp(const vector*>& bottom, vector*>* top) { @@ -73,11 +130,12 @@ void VideoDataLayer::DataLayerSetUp(const vector*>& bottom, "new_height and new_width to be set at the same time."; // Read the file with filenames and labels const string& source = this->layer_param_.video_data_param().source(); + setPerspective(); + + LOG(INFO) << "Opening schedule file " << source; std::ifstream infile(source.c_str()); - string batch_string; - string filename; //while (infile >> batch_string) { while (getline (infile, batch_string)) { @@ -212,7 +270,6 @@ void VideoDataLayer::InternalThreadEntry() { // Apply transformations (mirror, crop...) to the data this->data_transformer_.Transform(item_id, datum, this->mean_, top_data); - // go to the next iter lines_id_++; if (lines_id_ >= lines_size) { diff --git a/src/caffe/proto/caffe.proto b/src/caffe/proto/caffe.proto index 0e69ef54e77..3bbf33f70c0 100644 --- a/src/caffe/proto/caffe.proto +++ b/src/caffe/proto/caffe.proto @@ -219,7 +219,7 @@ message LayerParameter { // line above the enum. Update the next available ID when you add a new // LayerType. // - // LayerType next available ID: 39 (last added: VIDEO_DATA) + // LayerType next available ID: 40 (last added: MULTILANE_LABEL) enum LayerType { // "NONE" layer type is 0th enum element so that we don't cause confusion // by defaulting to an existent LayerType (instead, should usually error if @@ -247,6 +247,7 @@ message LayerParameter { INNER_PRODUCT = 14; LRN = 15; MEMORY_DATA = 29; + MULTILANE_LABEL = 39; MULTINOMIAL_LOGISTIC_LOSS = 16; MVN = 34; POOLING = 17; @@ -308,6 +309,7 @@ message LayerParameter { optional InnerProductParameter inner_product_param = 17; optional LRNParameter lrn_param = 18; optional MemoryDataParameter memory_data_param = 22; + optional MultilaneLabelParameter multilane_label_param = 42; optional MVNParameter mvn_param = 34; optional PoolingParameter pooling_param = 19; optional PowerParameter power_param = 21; @@ -526,6 +528,35 @@ message ImageDataParameter { optional bool mirror = 6 [default = false]; } +// Message that stores parameters used by MultilaneLabelLayer +message MultilaneLabelParameter { + // Specify the data source. + optional string source = 1; + // Specify the batch size. + optional uint32 batch_size = 4; + // The rand_skip variable is for the data layer to skip a few data points + // to avoid all asynchronous sgd clients to start at the same point. The skip + // point would be set as rand_skip * rand(0,1). Note that rand_skip should not + // be larger than the number of keys in the leveldb. + optional uint32 rand_skip = 7 [default = 0]; + // Whether or not MultilaneLabelLayer should shuffle the list of files at every epoch. + optional bool shuffle = 8 [default = false]; + // It will also resize images if new_height or new_width are not zero. + optional uint32 new_height = 9 [default = 0]; + optional uint32 new_width = 10 [default = 0]; + // DEPRECATED. See TransformationParameter. For data pre-processing, we can do + // simple scaling and subtracting the data mean, if provided. Note that the + // mean subtraction is always carried out before scaling. + optional float scale = 2 [default = 1]; + optional string mean_file = 3; + // DEPRECATED. See TransformationParameter. Specify if we would like to randomly + // crop an image. + optional uint32 crop_size = 5 [default = 0]; + // DEPRECATED. See TransformationParameter. Specify if we want to randomly mirror + // data. + optional bool mirror = 6 [default = false]; +} + // Message that stores parameters used by VideoDataLayer message VideoDataParameter { // Specify the data source. @@ -554,7 +585,6 @@ message VideoDataParameter { // data. optional bool mirror = 6 [default = false]; } - // Message that stores parameters InfogainLossLayer message InfogainLossParameter { // Specify the infogain matrix source. diff --git a/src/caffe/py_lane_label_reader/ArgParser.py b/src/caffe/py_lane_label_reader/ArgParser.py new file mode 100644 index 00000000000..0db65b696b5 --- /dev/null +++ b/src/caffe/py_lane_label_reader/ArgParser.py @@ -0,0 +1,34 @@ +from Q50_config import LoadParameters + +def parse_args(folder, video_file): + """Folder is the top level folder. +video_file is the name of the camera split to use +ex: parse_args('data/280S', '280S_a2.avi') +returns a dictionary with keys: ('map', 'frames', 'radar' 'gps', 'video', 'params') +corresponding to file names. +""" + + basename = folder + '/' + video_file[:-5] + + map_file = basename + '.map' + lidar_folder = basename + '_frames/' + radar_folder = basename + '_rdr/' + gps_file = basename + '_gps.out' + gps_mark1_file = basename + '_gpsmark1.out' + gps_mark2_file = basename + '_gpsmark2.out' + + video_file_num = video_file[-5] + + video_file = folder + '/' + video_file + param_file = folder + '/params.ini' + params = open(param_file, 'r').readline().rstrip() + + return {'map': map_file, + 'frames': lidar_folder, + 'radar': radar_folder, + 'gps': gps_file, + 'gps_mark1': gps_mark1_file, + 'gps_mark2': gps_mark2_file, + 'video': video_file, + 'cam_num': int(video_file[-5]), + 'params': LoadParameters(params)} diff --git a/src/caffe/py_lane_label_reader/CameraParams.py b/src/caffe/py_lane_label_reader/CameraParams.py new file mode 100644 index 00000000000..275c437f0c5 --- /dev/null +++ b/src/caffe/py_lane_label_reader/CameraParams.py @@ -0,0 +1,48 @@ +import numpy as np +import pickle +import sys +from WGS84toENU import * + +__all__ = [ "getCameraParams" ] + + +def getCameraParams(): + cam = [{}, {}] + for i in [0, 1]: + cam[i]['R_to_c_from_i'] = np.array([[-1, 0, 0], + [0, 0, -1], + [0, -1, 0]]) + + if i == 0: + cam[i]['rot_x'] = deg2rad(-0.8) # better cam 1 + cam[i]['rot_y'] = deg2rad(-0.5) + cam[i]['rot_z'] = deg2rad(-0.005) + cam[i]['t_x'] = -0.5 + cam[i]['t_y'] = 1.1 + cam[i]['t_z'] = 0.0 + elif i == 1: + cam[i]['rot_x'] = deg2rad(-0.61) # better cam 2 + cam[i]['rot_y'] = deg2rad(0.2) + cam[i]['rot_z'] = deg2rad(0.0) + cam[i]['t_x'] = 0.5 + cam[i]['t_y'] = 1.1 + cam[i]['t_z'] = 0.0 + + cam[i]['fx'] = 2221.8 + cam[i]['fy'] = 2233.7 + cam[i]['cu'] = 623.7 + cam[i]['cv'] = 445.7 + cam[i]['KK'] = np.array([[cam[i]['fx'], 0.0, cam[i]['cu']], + [0.0, cam[i]['fy'], cam[i]['cv']], + [0.0, 0.0, 1.0]]) + cam[i]['f'] = (cam[i]['fx'] + cam[i]['fy']) / 2 + + return cam + +if __name__ == '__main__': + cam= getCameraParams() + filename = 'cam_params.pickle' + if len(sys.argv) > 1: + filename = sys.argv[1] + pickle.dump(cam, open(filename, 'wb')) + diff --git a/src/caffe/py_lane_label_reader/CameraReprojection.py b/src/caffe/py_lane_label_reader/CameraReprojection.py new file mode 100644 index 00000000000..85ab2657169 --- /dev/null +++ b/src/caffe/py_lane_label_reader/CameraReprojection.py @@ -0,0 +1,36 @@ +import numpy as np +from numpy import sin, cos +from transformations import euler_matrix +""" +pixels is Nx2 matrix of coordinates, +cam is camera structure +""" +''' +recover 3d position of points on the ground surface close to the car, assumes fixed +camera height and flat road, so won't work for points in distances. +''' +def pixelTo3d(pixels, cam): + height = 1.105 # constants based on camera setup + R_camera_pitch = euler_matrix(cam['rot_x'], cam['rot_y'], cam['rot_z'], 'sxyz')[0:3,0:3] + KK = cam['KK'] + N = pixels.shape[0] + assert(pixels.shape[1] == 2) + # compute X/Z and Y/Z using inv(KK*R)*pixels + Pos_S = np.linalg.solve(np.dot(KK,R_camera_pitch), np.concatenate((pixels,np.ones([N,1])), axis=1).transpose()).transpose() + Y = np.ones((N))*height + Z = Y/Pos_S[:,1] + X = Pos_S[:,0]*Z + return np.vstack((X,Y,Z)).transpose() + +''' +recover 3d position of points that are at fixed Z distances +''' +def recover3d(pixels, cam, Z=np.array([10,20,30,40,50,60,70,80])): + KK = cam['KK'] + N = pixels.shape[0] + assert(pixels.shape[1] == 2) + # compute X/Z and Y/Z using inv(KK*R)*pixels + Pos_S = np.linalg.solve(KK, np.concatenate((pixels,np.ones([N,1])), axis=1).transpose()).transpose() + Y = Pos_S[:,1]*Z + X = Pos_S[:,0]*Z + return np.vstack((X,Y,Z)).transpose() diff --git a/src/caffe/py_lane_label_reader/GPSReader.py b/src/caffe/py_lane_label_reader/GPSReader.py new file mode 100644 index 00000000000..6c32ce5edb6 --- /dev/null +++ b/src/caffe/py_lane_label_reader/GPSReader.py @@ -0,0 +1,102 @@ +import numpy as np +from scipy.ndimage.filters import convolve1d + +def smoothData(arr, wind=51): + filt = np.ones(wind) / wind + return convolve1d(arr, filt, mode='nearest') + +def integrateVelocity(v_arr, d0): + return d0+ np.cumsum(v_arr) + +def smoothDerivativeData(arr, wind=51): + darr = arr[1:] - arr[:-1] + darr = smoothData(darr, wind) + return integrateVelocity(darr, arr[0]) + +def holdDerivativeData(arr, clipVal=0.1): + darr = arr[1:] - arr[:-1] + darr = np.clip(darr, -clipVal, clipVal) + return integrateVelocity(darr,arr[0]) + +class GPSCovarianceReader(): + def __init__(self,filename): + from math import sqrt + f = open(filename); + self.nextLineInsCov = False + self.data = [ ] + self.token_order = ['tx','ty','tz','rx','ry','rz'] + for line in f: + if 'INSCOV' in line: + self.nextLineInsCov = True + elif self.nextLineInsCov: + record = { } + self.nextLineInsCov = False + tokens = line.split(' ') + offset = 7 + record['tx'] = sqrt(float(tokens[offset + 0])) + record['ty'] = sqrt(float(tokens[offset + 4])) + record['tz'] = sqrt(float(tokens[offset + 8])) + offset += 9 + record['rx'] = sqrt(float(tokens[offset + 0])) + record['ry'] = sqrt(float(tokens[offset + 4])) + record['rz'] = sqrt(float(tokens[offset + 8])) + self.data.append(record) + + def getNumericData(self): + arr = np.zeros([len(self.data), 6], np.float64); + for t in range(len(self.data)): + for j in range(6): + arr[t,j] = self.data[t][self.token_order[j]] + return arr + +class GPSReader(): + def __init__(self, filename): + self.data = [ ] + self.token_order = ['seconds', 'lat', 'long', 'height', + 'v_north', 'v_east', 'v_up', 'rot_y', 'rot_x', + 'azimuth', 'week']; + f = open(filename); + for line in f: + l = line.rstrip() + tokens = l.split(',') + for k in range(len(tokens)): + if 'PVA' in tokens[k]: + break + tokens = tokens[k:] + # for TAGGEDMARKxPVA logs, delete the tag, and then process as is + #print tokens + if len(tokens) == 22: + del tokens[11] # eventID token + if len(tokens) == 21: + record = { } + for idx in range(10): + record[self.token_order[idx]] = float(tokens[idx+10]) + # Parse out the gps week + record[self.token_order[10]] = long((tokens[9].split(';'))[1]) + self.data.append(record) + + def getData(self): + return self.data; + + def getNumericData(self): + arr = np.zeros([len(self.data), 11], np.float64); + for t in range(len(self.data)): + for j in range(11): + arr[t,j] = self.data[t][self.token_order[j]] + + + #arr[:, 1] = smoothData(arr[:, 1]) + #arr[:, 2] = smoothData(arr[:, 2]) + #arr[:, 3] = smoothData(arr[:, 3]) + #arr[:-1, 3] = smoothDerivativeData(arr[:, 3], wind=251) + #arr[:-1, 1] = holdDerivativeData(arr[:, 1]) + #arr[:-1, 2] = holdDerivativeData(arr[:, 2]) + #arr[0:-1, 3] = holdDerivativeData(arr[:, 3], clipVal=0.1) + #arr[:, 3] = smoothData(arr[:, 3]) + #arr[:, 7] = smoothData(arr[:, 7]) + #arr[:, 8] = smoothData(arr[:, 8]) + #arr[:, 9] = smoothData(arr[:, 9]) + + return arr; + + diff --git a/src/caffe/py_lane_label_reader/GPSReprojection.py b/src/caffe/py_lane_label_reader/GPSReprojection.py new file mode 100644 index 00000000000..e512f886411 --- /dev/null +++ b/src/caffe/py_lane_label_reader/GPSReprojection.py @@ -0,0 +1,285 @@ +from transformations import euler_matrix +import numpy as np +from WGS84toENU import * +from numpy import array, dot, zeros, around, divide, ones +from LidarTransforms import * +from Q50_config import * +from cv2 import projectPoints +# only works for new Q50 data. Maps positions wrt imu_0 to cam_t +def MapPos(map_pos, imu_transforms_t, cam, T_from_i_to_l): + pts_wrt_imu_0 = array(map_pos).transpose() + pts_wrt_imu_0 = np.vstack((pts_wrt_imu_0, np.ones((1,pts_wrt_imu_0.shape[1])))) + # transform points from imu_0 to imu_t + pts_wrt_imu_t = np.dot( np.linalg.inv(imu_transforms_t), pts_wrt_imu_0) + # transform points from imu_t to lidar_t + pts_wrt_lidar_t = np.dot(T_from_i_to_l, pts_wrt_imu_t); + # transform points from lidar_t to camera_t + pts_wrt_camera_t = pts_wrt_lidar_t.transpose()[:, 0:3] + cam['displacement_from_l_to_c_in_lidar_frame'] + pts_wrt_camera_t = dot(R_to_c_from_l(cam), pts_wrt_camera_t.transpose()) + pts_wrt_camera_t = np.vstack((pts_wrt_camera_t, + np.ones((1,pts_wrt_camera_t.shape[1])))) + pts_wrt_camera_t = dot(cam['E'], pts_wrt_camera_t) + pts_wrt_camera_t = pts_wrt_camera_t[0:3,:] + return pts_wrt_camera_t + +# only works for new Q50 data. Maps vectors wrt imu_0 to cam_t +def MapVec(map_vec, imu_transforms_t, cam, T_from_i_to_l): + vec_wrt_imu_0 = array(map_vec).transpose() + # transform vectors from imu_0 to imu_t + vec_wrt_imu_t = np.dot(imu_transforms_t[0:3,0:3].transpose(), vec_wrt_imu_0) + # transform vectors from imu_t to lidar_t + vec_wrt_lidar_t = np.dot(T_from_i_to_l[0:3,0:3], vec_wrt_imu_t); + # transform vectors from lidar_t to camera_t + vec_wrt_camera_t = dot(R_to_c_from_l(cam), vec_wrt_lidar_t) + vec_wrt_camera_t = dot(cam['E'][0:3,0:3], vec_wrt_camera_t) + return vec_wrt_camera_t + +# same as MapPos, but takes the imu transforms of the car and computes the local trajectory on ground. +def MapPosTrajectory(imu_tr, imu_transforms_t, cam, T_from_i_to_l, height): + height_array = np.zeros([3,3]) + height_array[2,0]=-height + aa = np.dot(imu_tr[:,0:3,0:3], height_array)[:,:,0] # shift down in the self frame + pts_wrt_imu_0 = (array(imu_tr[:,0:3,3])+aa).transpose() + pts_wrt_imu_0 = np.vstack((pts_wrt_imu_0, np.ones((1,pts_wrt_imu_0.shape[1])))) + # transform points from imu_0 to imu_t + pts_wrt_imu_t = np.dot( np.linalg.inv(imu_transforms_t), pts_wrt_imu_0) + # transform points from imu_t to lidar_t + pts_wrt_lidar_t = np.dot(T_from_i_to_l, pts_wrt_imu_t); + # transform points from lidar_t to camera_t + pts_wrt_camera_t = pts_wrt_lidar_t.transpose()[:, 0:3] + cam['displacement_from_l_to_c_in_lidar_frame'] + pts_wrt_camera_t = dot(R_to_c_from_l(cam), pts_wrt_camera_t.transpose()) + pts_wrt_camera_t = np.vstack((pts_wrt_camera_t, + np.ones((1,pts_wrt_camera_t.shape[1])))) + pts_wrt_camera_t = dot(cam['E'], pts_wrt_camera_t) + pts_wrt_camera_t = pts_wrt_camera_t[0:3,:] + return pts_wrt_camera_t + + + +def IMUPosTrajectory(imu_tr, imu_transforms_t, cam, height): + height_array = np.zeros([3,3]) + height_array[2,0]=-height + aa = np.dot(imu_tr[:,0:3,0:3], height_array)[:,:,0] # shift down in the self frame + pts_wrt_imu_0 = (array(imu_tr[:,0:3,3])+aa).transpose() + pts_wrt_imu_0 = np.vstack((pts_wrt_imu_0, np.ones((1,pts_wrt_imu_0.shape[1])))) + # transform points from imu_0 to imu_t + pts_wrt_imu_t = np.dot( np.linalg.inv(imu_transforms_t), pts_wrt_imu_0) + return pts_wrt_imu_t[0:3] + +def ENU2IMUQ50(world_coordinates, start_frame): + roll_start = deg2rad(start_frame[8]); + pitch_start = deg2rad(start_frame[7]); + yaw_start = -deg2rad(start_frame[9]); + + psi = pitch_start; + cp = cos(psi); + sp = sin(psi); + theta = roll_start; + ct = cos(theta); + st = sin(theta); + gamma = yaw_start; + cg = cos(gamma); + sg = sin(gamma); + + R_to_i_from_w = \ + array([[cg*cp-sg*st*sp, -sg*ct, cg*sp+sg*st*cp], + [sg*cp+cg*st*sp, cg*ct, sg*sp-cg*st*cp], + [-ct*sp, st, ct*cp]]).transpose() + pos_wrt_imu = dot(R_to_i_from_w, world_coordinates); + return pos_wrt_imu + + + +def GPSVelocities(GPSData): + return (np.apply_along_axis(np.linalg.norm, 1, GPSData[:,4:7])) + +def GPSPos(GPSData, Camera, start_frame): + roll_start = -deg2rad(start_frame[7]); + pitch_start = deg2rad(start_frame[8]); + yaw_start = -deg2rad(start_frame[9]+90); + + psi = pitch_start; + cp = cos(psi); + sp = sin(psi); + theta = roll_start; + ct = cos(theta); + st = sin(theta); + gamma = yaw_start; + cg = cos(gamma); + sg = sin(gamma); + + R_to_i_from_w = \ + array([[cg*cp-sg*st*sp, -sg*ct, cg*sp+sg*st*cp], + [sg*cp+cg*st*sp, cg*ct, sg*sp-cg*st*cp], + [-ct*sp, st, ct*cp]]).transpose() + + + pts = WGS84toENU(start_frame[1:4], GPSData[:, 1:4]) + world_coordinates = pts; + pos_wrt_imu = dot(R_to_i_from_w, world_coordinates); + R_to_c_from_i = Camera['R_to_c_from_i'] + R_camera_pitch = euler_matrix(Camera['rot_x'], Camera['rot_y'],\ + Camera['rot_z'], 'sxyz')[0:3,0:3] + R_to_c_from_i = dot(R_camera_pitch, R_to_c_from_i) + + pos_wrt_camera = dot(R_to_c_from_i, pos_wrt_imu); + + pos_wrt_camera[0,:] += Camera['t_x'] #move to left/right + pos_wrt_camera[1,:] += Camera['t_y'] #move up/down image + pos_wrt_camera[2,:] += Camera['t_z'] #move away from cam + return pos_wrt_camera + + + +def GPSPosShifted(GPSData, Camera, start_frame): + roll_start = -deg2rad(start_frame[7]); + pitch_start = deg2rad(start_frame[8]); + yaw_start = -deg2rad(start_frame[9]+90); + + psi = pitch_start; + cp = cos(psi); + sp = sin(psi); + theta = roll_start; + ct = cos(theta); + st = sin(theta); + gamma = yaw_start; + cg = cos(gamma); + sg = sin(gamma); + + R_to_i_from_w = \ + array([[cg*cp-sg*st*sp, -sg*ct, cg*sp+sg*st*cp], + [sg*cp+cg*st*sp, cg*ct, sg*sp-cg*st*cp], + [-ct*sp, st, ct*cp]]).transpose() + + + pts = WGS84toENU(start_frame[1:4], GPSData[:, 1:4]) + vel = GPSData[:,4:7] + vel[:,[0, 1]] = vel[:,[1, 0]] + sideways = np.cross(vel, np.array([0,0,1]), axisa=1) + sideways/= np.sqrt((sideways ** 2).sum(-1))[..., np.newaxis] + #pts[0,:] = pts[0,:]+np.transpose(GPSData[:,4])/40 + #pts[1,:] = pts[1,:]-np.transpose(GPSData[:,5])/40 + pts = pts+sideways.transpose() + world_coordinates = pts; + pos_wrt_imu = dot(R_to_i_from_w, world_coordinates); + R_to_c_from_i = Camera['R_to_c_from_i'] + R_camera_pitch = euler_matrix(Camera['rot_x'], Camera['rot_y'],\ + Camera['rot_z'], 'sxyz')[0:3,0:3] + R_to_c_from_i = dot(R_camera_pitch, R_to_c_from_i) + + pos_wrt_camera = dot(R_to_c_from_i, pos_wrt_imu); + + pos_wrt_camera[0,:] += Camera['t_x'] #move to left/right + pos_wrt_camera[1,:] += Camera['t_y'] #move up/down image + pos_wrt_camera[2,:] += Camera['t_z'] #move away from cam + return pos_wrt_camera + + + +def ENU2IMU(world_coordinates, start_frame): + roll_start = -deg2rad(start_frame[7]); + pitch_start = deg2rad(start_frame[8]); + yaw_start = -deg2rad(start_frame[9]+90); + + psi = pitch_start; + cp = cos(psi); + sp = sin(psi); + theta = roll_start; + ct = cos(theta); + st = sin(theta); + gamma = yaw_start; + cg = cos(gamma); + sg = sin(gamma); + + R_to_i_from_w = \ + array([[cg*cp-sg*st*sp, -sg*ct, cg*sp+sg*st*cp], + [sg*cp+cg*st*sp, cg*ct, sg*sp-cg*st*cp], + [-ct*sp, st, ct*cp]]).transpose() + pos_wrt_imu = dot(R_to_i_from_w, world_coordinates); + return pos_wrt_imu + + +def GPSPosIMU(GPSData, Camera, start_frame): + roll_start = -deg2rad(start_frame[7]); + pitch_start = deg2rad(start_frame[8]); + yaw_start = -deg2rad(start_frame[9]+90); + + psi = pitch_start; + cp = cos(psi); + sp = sin(psi); + theta = roll_start; + ct = cos(theta); + st = sin(theta); + gamma = yaw_start; + cg = cos(gamma); + sg = sin(gamma); + + R_to_i_from_w = \ + array([[cg*cp-sg*st*sp, -sg*ct, cg*sp+sg*st*cp], + [sg*cp+cg*st*sp, cg*ct, sg*sp-cg*st*cp], + [-ct*sp, st, ct*cp]]).transpose() + pts = WGS84toENU(start_frame[1:4], GPSData[:, 1:4]) + world_coordinates = pts; + pos_wrt_imu = dot(R_to_i_from_w, world_coordinates); + return pos_wrt_imu + +def GPSPosCamera(pos_wrt_imu, Camera): + R_to_c_from_i = Camera['R_to_c_from_i'] + R_camera_pitch = euler_matrix(Camera['rot_x'], Camera['rot_y'],Camera['rot_z'], 'sxyz')[0:3,0:3] + R_to_c_from_i = dot(R_camera_pitch, R_to_c_from_i) + + pos_wrt_camera = dot(R_to_c_from_i, pos_wrt_imu); + + pos_wrt_camera[0,:] += Camera['t_x'] #move to left/right + pos_wrt_camera[1,:] += Camera['t_y'] #move up/down image + pos_wrt_camera[2,:] += Camera['t_z'] #move away from cam + return pos_wrt_camera + + + + +def GPSColumns(GPSData, Camera, start_frame): + pos_wrt_camera = GPSPos(GPSData, Camera, start_frame) + return PointsMask(pos_wrt_camera[:,1:], Camera) + +def GPSShiftedColumns(GPSData, Camera, start_frame): + pos_wrt_camera = GPSPosShifted(GPSData, Camera, start_frame) + return PointsMask(pos_wrt_camera, Camera) + +def PointsMask(pos_wrt_camera, Camera): + if False:#'distort' in Camera: + (vpix, J) = projectPoints(pts_wrt_camera.transpose(), np.array([0.0,0.0,0.0]), np.array([0.0,0.0,0.0]), Camera['KK'], Camera['distort']) + else: + vpix = dot(Camera['KK'], divide(pos_wrt_camera, pos_wrt_camera[2,:])) + vpix = around(vpix).astype(np.int32) + return vpix + + +def GPSMask(GPSData, Camera, width=2): + I = 255*ones((960,1280,3), np.uint8) + vpix = GPSColumns(GPSData, Camera, GPSData[0, :]) + vpix = vpix[:,vpix[0,:] > 0 + width/2] + if vpix.size > 0: + vpix = vpix[:,vpix[1,:] > 0 + width/2] + if vpix.size > 0: + vpix = vpix[:,vpix[0,:] < 1279 - width/2] + if vpix.size > 0: + vpix = vpix[:,vpix[1,:] < 959 - width/2] + + for p in range(-width/2,width/2): + I[vpix[1,:]+p, vpix[0,:], :] = 0 + I[vpix[1,:], vpix[0,:]+p, :] = 0 + I[vpix[1,:]-p, vpix[0,:], :] = 0 + I[vpix[1,:], vpix[0,:]-p, :] = 0 + + """ + for idx in range(1,pts.shape[1]): + pix = vpix[:,idx] + if (pix[0] > 0 and pix[0] < 1280 and pix[1] > 0 and pix[1] < 960): + I[pix[1]-width+1:pix[1]+width, pix[0]-width+1:pix[0]+width, 0] = 0; + I[pix[1]-width+1:pix[1]+width, pix[0]-width+1:pix[0]+width, 1] = 0; + I[pix[1]-width+1:pix[1]+width, pix[0]-width+1:pix[0]+width, 2] = 0; + """ + + return I + diff --git a/src/caffe/py_lane_label_reader/GPSTransforms.py b/src/caffe/py_lane_label_reader/GPSTransforms.py new file mode 100644 index 00000000000..edc4504754e --- /dev/null +++ b/src/caffe/py_lane_label_reader/GPSTransforms.py @@ -0,0 +1,109 @@ +from transformations import euler_matrix +import numpy as np +from WGS84toENU import * +from numpy import array, dot, zeros, around, divide, ones +import bisect + + + +# get transformation id using time +def Idfromt(gps_times, t): + idx = bisect.bisect(gps_times, t) - 1 + return idx + + +def R_to_i_from_w(roll, pitch, yaw): + cp = cos(pitch) + sp = sin(pitch) + ct = cos(roll) + st = sin(roll) + cg = cos(yaw) + sg = sin(yaw) + + R_to_i_from_w = \ + array([[cg*cp-sg*st*sp, -sg*ct, cg*sp+sg*st*cp], + [sg*cp+cg*st*sp, cg*ct, sg*sp-cg*st*cp], + [-ct*sp, st, ct*cp]]).transpose() + + return R_to_i_from_w; + +def R_to_c_from_i(cam): + R_to_c_from_i = cam['R_to_c_from_i'] + R_camera_pitch = euler_matrix(cam['rot_x'], cam['rot_y'], cam['rot_z'], 'sxyz')[0:3,0:3] + R_to_c_from_i = dot(R_camera_pitch, R_to_c_from_i) + + return R_to_c_from_i + +def R_to_c_from_w(roll, pitch, yaw, cam): + return dot(R_to_c_from_i(cam), R_to_i_from_w(roll, pitch, yaw) ) + + +def integrateVelocity(vel, dt=1/50.0): + return np.cumsum(vel*dt, axis=0) + +def smoothCoordinates(GPSData): + tr = np.array([np.eye(4),]*GPSData.shape[0]) + + roll_start = deg2rad(GPSData[0,8]) + pitch_start = deg2rad(GPSData[0,7]) + yaw_start = -deg2rad(GPSData[0,9]) + base_R_to_i_from_w = R_to_i_from_w(roll_start, pitch_start, yaw_start) + + NEU_coords = integrateVelocity(GPSData[:,4:7]) + ENU_coords = np.array(NEU_coords) + ENU_coords[:,0] = NEU_coords[:,1] + ENU_coords[:,1] = NEU_coords[:,0] + ENU_coords[:,2] = NEU_coords[:,2] + + pos_wrt_imu = np.dot(base_R_to_i_from_w, ENU_coords.transpose()) + + tr[:,0,3] = pos_wrt_imu[0,:] + tr[:,1,3] = pos_wrt_imu[1,:] + tr[:,2,3] = pos_wrt_imu[2,:] + + for i in xrange(GPSData.shape[0]): + roll = deg2rad(GPSData[i,8]) + pitch = deg2rad(GPSData[i,7]) + yaw = -deg2rad(GPSData[i,9]) + + rot = R_to_i_from_w(roll, pitch, yaw).transpose() + tr[i, 0:3,0:3] = dot(base_R_to_i_from_w, rot) + + return tr + + +""" +returns the relative transformations of the IMU from time 0 to the end of the GPS log. +""" +def IMUTransforms(GPSData): + return smoothCoordinates(GPSData) + +def GPSTransforms(GPSData, Camera, width=2): + + tr = np.array([np.eye(4),]*GPSData.shape[0]) + + roll_start = -deg2rad(GPSData[0,7]); + pitch_start = deg2rad(GPSData[0,8]); + yaw_start = -deg2rad(GPSData[0,9]+90); + + base_R_to_c_from_w = R_to_c_from_w(roll_start, pitch_start, yaw_start, Camera) + pts = WGS84toENU(GPSData[0,1:4], GPSData[:,1:4]) + + world_coordinates = pts; + pos_wrt_camera = dot(base_R_to_c_from_w, world_coordinates); + + tr[:,0,3] = pos_wrt_camera[0,:] + tr[:,1,3] = pos_wrt_camera[1,:] + tr[:,2,3] = pos_wrt_camera[2,:] + + for i in xrange(GPSData.shape[0]): + roll = -deg2rad(GPSData[i,7]) + pitch = deg2rad(GPSData[i,8]) + yaw = -deg2rad(GPSData[i,9] + 90) + + rot = R_to_c_from_w(roll, pitch, yaw, Camera).transpose() + + pos = pos_wrt_camera[0:3,i] + tr[i, 0:3,0:3] = dot(base_R_to_c_from_w, rot) + + return tr diff --git a/src/caffe/py_lane_label_reader/LidarTransforms.py b/src/caffe/py_lane_label_reader/LidarTransforms.py new file mode 100644 index 00000000000..bd672fdfb90 --- /dev/null +++ b/src/caffe/py_lane_label_reader/LidarTransforms.py @@ -0,0 +1,192 @@ +import os +import glob +import numpy as np +import bisect +from transformations import quaternion_from_matrix, quaternion_slerp,\ + quaternion_matrix + +# PARAM +SWEEP_TIME_MICROSEC = 100000 + + +def loadLDR(ldrfile): + z = np.fromfile(ldrfile, dtype=np.float32) + z = z.reshape((z.shape[0] / 6, 6)) + return z + +def loadLDRCamMap(frame_cloud_map): + map_file = open(frame_cloud_map, 'r') + clouds = [] + frame_folder, map_name = os.path.split(frame_cloud_map) + frame_folder = frame_folder + '/' + map_name.split('.')[0] + '_frames' + + for line in map_file: + ldr_file = line.rstrip().split(' ')[1] + clouds.append(frame_folder + '/' + ldr_file) + map_file.close() + + return clouds + +def utc_from_gps_log(log): + return utc_from_gps(log[10], log[0]) + + +def utc_from_gps_log_all(log): + return utc_from_gps(log[:, 10], log[:, 0]) + + +def utc_from_gps(gps_week, seconds, leap_seconds=16): + """ Converts from gps week time to UTC time. UTC time starts from JAN 1, + 1970 and GPS time starts from JAN 6, 1980. + + http://leapsecond.com/java/gpsclock.htm + """ + + secs_in_week = 604800 + secs_gps_to_utc = 315964800 + + utc = (gps_week * secs_in_week + seconds + secs_gps_to_utc - + leap_seconds) * 1000000 + if type(gps_week) is np.ndarray: + return np.array(utc, dtype=np.int64) + else: + return long(utc) + + +class LDRLoader(object): + + def __init__(self, ldr_dir): + ldr_files = sorted(list(glob.glob(os.path.join(ldr_dir, '*.ldr')))) + file_times = np.array([int(os.path.splitext(os.path.basename(f))[0]) for f in ldr_files], dtype=np.int64) + sweep_end_times = file_times + sweep_start_times = sweep_end_times - SWEEP_TIME_MICROSEC + self.ldr_files = np.array(ldr_files) + self.sweep_start_times = sweep_start_times + self.sweep_end_times = sweep_end_times + + def loadLDRWindow(self, microsec_since_epoch, time_window_sec): + time_window = time_window_sec * 1e6 + + # Get files that may have points within the time window + mask = (self.sweep_start_times >= microsec_since_epoch - time_window / 2.0 - SWEEP_TIME_MICROSEC) &\ + (self.sweep_end_times <= microsec_since_epoch + time_window / 2.0 + SWEEP_TIME_MICROSEC) + ldr_files = self.ldr_files[mask].tolist() + #print ldr_files + sweep_end_times = self.sweep_end_times[mask].tolist() + + # Load the points from those times + all_data = None + all_times = None + for (ldr_file, sweep_end_time) in zip(ldr_files, sweep_end_times): + data = loadLDR(ldr_file) + times = -1 * np.array(data[:, 5], dtype=np.int64) + sweep_end_time + + if all_data is None: + all_data = data + all_times = times + else: + all_data = np.vstack((all_data, data)) + all_times = np.concatenate((all_times, times)) + + # Filter points within time window + + mask = (all_times >= microsec_since_epoch - time_window / 2.0) &\ + (all_times <= microsec_since_epoch + time_window / 2.0) + if all_data is None: + return None, None + return all_data[mask, :], all_times[mask] + + +def R_to_c_from_l_old(cam): + # hard coded calibration parameters for now + R_to_c_from_l = np.array([[0.0, -1.0, 0.0], + [0.0, 0.0, -1.0], + [1.0, 0.0, 0.0]]) + + return R_to_c_from_l + +def R_to_c_from_l(cam): + # hard coded calibration parameters for now + R_to_c_from_l = np.array([[0.0, -1.0, 0.0], + [0.0, 0.0, -1.0], + [1.0, 0.0, 0.0]]) + R_to_c_from_l = np.dot(cam['R_to_c_from_l_in_camera_frame'], R_to_c_from_l) + + return R_to_c_from_l + + +def interp_transforms(T1, T2, alpha): + assert alpha <= 1 + T_avg = alpha * T1 + (1 - alpha) * T2 + q1 = quaternion_from_matrix(T1) + q2 = quaternion_from_matrix(T2) + q3 = quaternion_slerp(q1, q2, alpha) + R = quaternion_matrix(q3) + T_avg[0:3, 0:3] = R[0:3, 0:3] + return T_avg + + +def interp_transforms_backward(imu_transforms, ind): + assert ind < 0, 'No need to call interp_transforms_backward' + T_interp = imu_transforms[0, :, :] -\ + (imu_transforms[-1 * ind, :, :] - imu_transforms[0, :, :]) + R = T_interp[0:3, 0:3] + R = np.linalg.qr(R, mode='complete')[0] + T_interp[0:3, 0:3] = R + return T_interp + + +def transform_points_in_sweep(pts, times, fnum, imu_transforms): + for time in set(times): + mask = times == time + + # FIXME PARAM + offset = (time / float(1e6)) / 0.05 + offset = min(5, offset) + + ind1 = int(fnum - math.ceil(offset)) + ind2 = int(fnum - math.floor(offset)) + + # FIXME Hack to interpolate before 0 + #ind1, ind2 = max(ind1, 0), max(ind2, 0) + if ind1 < 0: + T1 = interp_transforms_backward(imu_transforms, ind1) + else: + T1 = imu_transforms[ind1, :, :] + if ind2 < 0: + T2 = interp_transforms_backward(imu_transforms, ind2) + else: + T2 = imu_transforms[ind2, :, :] + + transform = interp_transforms(T1, T2, offset / 5.0) + + # transform data into imu_0 frame + pts[:, mask] = np.dot(transform, pts[:, mask]) + + +def transform_points_by_times(pts, t_pts, imu_transforms, gps_times): + for t in set(t_pts): + mask = t_pts == t + + fnum1 = bisect.bisect(gps_times, t) - 1 + fnum2 = fnum1 + 1 + try: + alpha = (1 - (t - gps_times[fnum1])) / float(gps_times[fnum2] - + gps_times[fnum1]) + except IndexError: + continue + + T1 = imu_transforms[fnum1, :, :] + T2 = imu_transforms[fnum2, :, :] + + transform = interp_transforms(T1, T2, alpha) + + # transform data into imu_0 frame + pts[:, mask] = np.dot(transform, pts[:, mask]) + + +if __name__ == '__main__': + import sys + z = loadLDRCamMap(sys.argv[1]) + for x in z: + print x diff --git a/src/caffe/py_lane_label_reader/Q50_config.py b/src/caffe/py_lane_label_reader/Q50_config.py new file mode 100644 index 00000000000..c2de9084088 --- /dev/null +++ b/src/caffe/py_lane_label_reader/Q50_config.py @@ -0,0 +1,9 @@ +def LoadParameters(name): + if name == 'q50_4_3_14_params': + from parameters.q50_4_3_14_params import GetQ50Params + return GetQ50Params() + elif name == 'q50_3_7_14_params': + from parameters.q50_3_7_14_params import GetQ50Params + return GetQ50Params() + else: + raise Exception("Parameter file " + name + " not found") diff --git a/src/caffe/py_lane_label_reader/SetPerspDist.py b/src/caffe/py_lane_label_reader/SetPerspDist.py new file mode 100644 index 00000000000..5fb058f75c3 --- /dev/null +++ b/src/caffe/py_lane_label_reader/SetPerspDist.py @@ -0,0 +1,49 @@ +import numpy as np +from transformations import euler_matrix +from transformations import translation_matrix +import pickle +# defines a set of perspective transforms that correspond to shifts/rotations in the real world + +def setPersp(scale_factor = 1, distort_location = '/afs/cs.stanford.edu/u/twangcat/scratch/sail-car-log/process/'): + distort = { } + distort['M_1'] = [np.eye(3)] # perspective distrotion in cam1 image space + distort['M_2'] = [np.eye(3)] # perspective distrotion in cam2 image space + distort['T'] = [np.eye(4)] # corresponding real-world transformation + + + distort['T'].insert(0,translation_matrix(np.array([1.0,0,0]))) + distort['T'].insert(0,translation_matrix(np.array([-1.0,0,0]))) + distort['T'].insert(0,euler_matrix(0, 0.06, 0)) + distort['T'].insert(0,euler_matrix(0, -0.06, 0)) + distort['T'].insert(0,euler_matrix(0, 0.12, 0)) + distort['T'].insert(0,euler_matrix(0, -0.12, 0)) + + + distort['M_1'].insert(0,pickle.load(open(distort_location+'M_shift1.0_cam1.pickle'))) # shift 1.0 + distort['M_1'].insert(0,pickle.load(open(distort_location+'M_shift-1.0_cam1.pickle'))) # shift -1.0 + distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot0.06_cam1.pickle','r'))) # rotate 0.06 + distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot-0.06_cam1.pickle','r'))) # rotate -0.06 + distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot0.12_cam1.pickle','r'))) # rotate 0.12 + distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot-0.12_cam1.pickle','r'))) # rotate -0.12 + + + + + distort['M_2'].insert(0,pickle.load(open(distort_location+'M_shift1.0_cam2.pickle'))) # shift 1.0 + distort['M_2'].insert(0,pickle.load(open(distort_location+'M_shift-1.0_cam2.pickle'))) # shift -1.0 + distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot0.06_cam2.pickle','r'))) # rotate 0.06 + distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot-0.06_cam2.pickle','r'))) # rotate -0.06 + distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot0.12_cam2.pickle','r'))) # rotate 0.12 + distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot-0.12_cam2.pickle','r'))) # rotate -0.12 + + assert len(distort['M_1']) == len(distort['M_2']) and len(distort['M_1'])==len(distort['T']) + if scale_factor != 1: + for i in xrange(len(distort['M_1'])-1): # last one is always identity, no need change. + distort['M_1'][i][:,0:2]/=scale_factor + distort['M_1'][i][2,:]/=scale_factor + distort['M_2'][i][:,0:2]/=scale_factor + distort['M_2'][i][2,:]/=scale_factor + + + return distort + diff --git a/src/caffe/py_lane_label_reader/WGS84toENU.py b/src/caffe/py_lane_label_reader/WGS84toENU.py new file mode 100644 index 00000000000..72bc967df9c --- /dev/null +++ b/src/caffe/py_lane_label_reader/WGS84toENU.py @@ -0,0 +1,66 @@ +# port of dllh2denu.m +from math import pi, cos, sin, sqrt +from numpy import array, power + +def WGS84toENU(llh0, llh): + ########CONSTANTS + a = 6378137; + b = 6356752.3142; + e2 = 1 - (b/a)**2; + ########Location of reference point in radians + phi = llh0[0]*pi/180; + lam = llh0[1]*pi/180; + h = llh0[2]; + ########Location of data points in radians + dphi = llh[:,0]*pi/180 - phi; + dlam = llh[:,1]*pi/180 - lam; + dh = llh[:,2] - h; + ########Some useful definitions + tmp1 = sqrt(1-e2*power(sin(phi),2.0)); + cl = cos(lam); + sl = sin(lam); + cp = cos(phi); + sp = sin(phi); + ########Transformations + de = (a/tmp1+h)*cp*dlam - (a*(1-e2)/(tmp1**3)+h)*sp*dphi*dlam +cp*dlam*dh; + dn = (a*(1-e2)/tmp1**3 + h)*dphi + 1.5*cp*sp*a*e2*power(dphi,2) + sp**2*dh*dphi \ + + 0.5*sp*cp*(a/tmp1 +h)*power(dlam,2); + du = dh - 0.5*(a-1.5*a*e2*cp**2+0.5*a*e2+h)*power(dphi,2) \ + - 0.5*cp**2*(a/tmp1 -h)*power(dlam,2); + denu = array([de, dn, du]); + return denu; + + +def WGS84toENU_slow(llh0, llh): + ########CONSTANTS + a = 6378137; + b = 6356752.3142; + e2 = 1 - (b/a)**2; + ########Location of reference point in radians + phi = llh0[0]*pi/180; + lam = llh0[1]*pi/180; + h = llh0[2]; + ########Location of data points in radians + dphi= llh[0]*pi/180 - phi; + dlam= llh[1]*pi/180 - lam; + dh = llh[2] - h; + ########Some useful definitions + tmp1 = sqrt(1-e2*sin(phi)**2); + cl = cos(lam); + sl = sin(lam); + cp = cos(phi); + sp = sin(phi); + ########Transformations + de = (a/tmp1+h)*cp*dlam - (a*(1-e2)/(tmp1**3)+h)*sp*dphi*dlam +cp*dlam*dh; + dn = (a*(1-e2)/tmp1**3 + h)*dphi + 1.5*cp*sp*a*e2*dphi**2 + sp**2*dh*dphi \ + + 0.5*sp*cp*(a/tmp1 +h)*dlam**2; + du = dh - 0.5*(a-1.5*a*e2*cp**2+0.5*a*e2+h)*dphi**2 \ + - 0.5*cp**2*(a/tmp1 -h)*dlam**2; + denu = array([de, dn, du]); + return denu; + +def deg2rad(angleInDeg): + return (pi / 180) * angleInDeg; + +def rad2deg(angleInRad): + return (180 / pi) * angleInRad; diff --git a/src/caffe/py_lane_label_reader/WarpUtils.py b/src/caffe/py_lane_label_reader/WarpUtils.py new file mode 100644 index 00000000000..c88cdf417cc --- /dev/null +++ b/src/caffe/py_lane_label_reader/WarpUtils.py @@ -0,0 +1,14 @@ +import cv, cv2 +import numpy as np + + +def warpPoints(P, pts): + """ + warpPoints takes a list of points and performs a matrix transform on them + + P is a perspective transform matrix (3x3) + pts is a 2xN matrix of (x, y) coordinates to be transformed + """ + pts = np.vstack([pts, np.ones((1, pts.shape[1]))]) + out = np.dot(P, pts) + return out[0:2] / out[2] diff --git a/src/caffe/py_lane_label_reader/__init__.py b/src/caffe/py_lane_label_reader/__init__.py new file mode 100644 index 00000000000..1227be3e9ee --- /dev/null +++ b/src/caffe/py_lane_label_reader/__init__.py @@ -0,0 +1,2 @@ +from multilane_label_reader import * +from perspective_reader import * diff --git a/src/caffe/py_lane_label_reader/multilane_label_reader.py b/src/caffe/py_lane_label_reader/multilane_label_reader.py new file mode 100644 index 00000000000..8643516c075 --- /dev/null +++ b/src/caffe/py_lane_label_reader/multilane_label_reader.py @@ -0,0 +1,404 @@ +from numpy import array, dot, zeros, around, divide, ones +import cv,cv2 +from transformations import euler_matrix +from copy import deepcopy +import collections +import os +import numpy as np +import logging +from threading import Thread +from Queue import Queue +import pickle +from GPSReader import GPSReader +from GPSTransforms import * +from GPSReprojection import * +from WarpUtils import warpPoints +from Q50_config import * +from ArgParser import * +from scipy.io import loadmat +import string +import time +from SetPerspDist import setPersp +import os +import time +__all__=['MultilaneLabelReader'] + + +blue = np.array([255,0,0]) +green = np.array([0,255,0]) +red = np.array([0,0,255]) + +def dist2color(dist, max_dist = 90.0): + # given a distance and a maximum distance, gives a color code for the distance. + # red being closest, green is mid-range, blue being furthest + alpha = (dist/max_dist) + if alpha<0.5: + color = red*(1-alpha*2)+green*alpha*2 + else: + beta = alpha-0.5 + color = green*(1-beta*2)+blue*beta*2 + return color.astype(np.int) + +def colorful_line(img, start, end, start_color, end_color, thickness): + # similar to cv.line, but draws a line with gradually (linearly) changing color. + # allows starting and ending color to be specified. + # implemented using recursion. + if ((start[0]-end[0])**2 + (start[1]-end[1])**2)**0.5<=thickness*2: + cv2.line(img, start, end ,start_color,thickness) + return img + mid = (int((start[0]+end[0])/2),int((start[1]+end[1])/2)) + mid_color = [int((start_color[0]+end_color[0])/2),int((start_color[1]+end_color[1])/2),int((start_color[2]+end_color[2]))/2] + img = colorful_line(img, start, mid, start_color, mid_color, thickness) + img = colorful_line(img, mid, end, mid_color, end_color, thickness) + return img + + + +class MultilaneLabelReader(): + def __init__(self, imdepth=3, imwidth=640, imheight=480, markingWidth=0.07, distortion_file='/scail/group/deeplearning/driving_data/perspective_transforms.pickle', pixShift=0, label_dim = [80,60], new_distort=False, predict_depth = False, readVideo=False): + self.new_distort = new_distort + if new_distort: + self.Ps = setPersp() + else: + self.Ps = pickle.load(open(distortion_file, 'rb')) + self.lane_values = dict() + self.gps_values1 = dict() + self.gps_values2 = dict() + self.count=0 + self.markingWidth = markingWidth + self.pixShift = pixShift + self.labelw = label_dim[0] + self.labelh = label_dim[1] + self.labeld = 6 if predict_depth else 4 + self.imwidth = imwidth + self.imheight = imheight + self.imdepth = imdepth + self.label_scale = None + self.img_scale = None + self.predict_depth = predict_depth + self.visualize = readVideo + self.griddim=4 + self.colors = [(255,0,0),(0,255,0),(0,0,255),(255,255,0),(255,0,255),(0,255,255),(128,128,255),(128,255,128),(255,128,128),(128,128,0),(128,0,128),(0,128,128),(0,128,255),(0,255,128),(128,0,255),(128,255,0),(255,0,128),(255,128,0)] + # arrays to adjust for rf offset + self.x_adj = (np.floor(np.arange(label_dim[0])/self.griddim)*self.griddim+self.griddim/2)*imwidth/label_dim[0] + self.y_adj = (np.floor(np.arange(label_dim[1])/self.griddim)*self.griddim+self.griddim/2)*imheight/label_dim[1] + self.y_adj = np.array([self.y_adj]).transpose() + + def zDistances(self, distances, global_frame, starting_point, meters_per_point, points_fwd): + output = [] + point_num = 1 + dist = 0 + for pt in xrange(points_fwd): + dist = pt * meters_per_point+starting_point + output.append((np.abs(distances-dist)).argmin()+global_frame) + return output + + + def dist2Id(self, dist, laneWidth=3.6576): # default lane width for US highways is 12 ft + return np.floor(np.abs(dist)/laneWidth)*2+int(dist>0) + + def setLaneIDs(self, lane, center2, sideways): + # go through each lane. Define lane id based on lateral dist at fixed longitudinal distances. + num_anchors = center2.shape[1] + #id_change=False # if the lane id changed during the course of this lane in the current frame + # define the anchor pts on this lane + anchors = np.empty([0,5]) + id=float('nan') + for n in range(num_anchors): + diff=lane-center2[:,n:n+1] + minid = np.argmin((diff**2).sum(0)) + mindist = np.dot(diff[:,minid],sideways[n,:]) # length projected to perpendicular vector + if (minid==0 or minid==lane.shape[1]-1) and np.abs(mindist/np.sqrt(np.sum(diff[:,minid]**2)))<0.95: + continue + else: + anchor_id = self.dist2Id(mindist) + temp = np.empty([1,5]) + #temp[0,0:3]=lane[:,minid] + temp[0,0:3]=center2[:,n]+mindist*sideways[n,:] + temp[0,3]=n + temp[0,4]=anchor_id + anchors = np.r_[anchors, temp] + if not np.isnan(id): + id = anchor_id + return lane,id, anchors + + + + def runBatch(self, vid_name, gps_dat, gps_times1, gps_times2, frames, start_frame, final_frame, lanes, tr1,Pid, split_num, cam_num, params): + if self.visualize: + print 'warning: reading videos in labeller...' + cap = cv2.VideoCapture(vid_name) + cam = params['cam'][cam_num-1]#self.cam[cam_num - 1] + lidar_height = params['lidar']['height'] + T_from_l_to_i = params['lidar']['T_from_l_to_i'] + T_from_i_to_l = np.linalg.inv(T_from_l_to_i) + starting_point = 4#12 + meters_per_point = 80#24#12#6 + points_fwd = 2#6#12 + starting_point2 = 15#12 + meters_per_point2 = 5#24#12#6 + points_fwd2 = 15#6#12 + scan_range = starting_point + (points_fwd-1)*meters_per_point + seconds_ahead=5 + output_num = 0 + batchSize = frames.shape[0] + labels= np.zeros([batchSize,self.labelh, self.labelw,1],dtype='f4',order='C') + reg_labels= np.zeros([batchSize, self.labelh, self.labelw,self.labeld],dtype='f4',order='C') + weight_labels= np.ones([batchSize, self.labelh, self.labelw,1],dtype='f4',order='C') + + labels_3d= [] + trajectory_3d= [] + count = 0 + #print 'reading labels... ', + labelling_time=0 + for idx in xrange(batchSize): + frame = frames[idx] + fnum2 =frame*10+split_num-1 # global video frame. if split0, *10+9; if split1, *10+0; if split 2, *10+1 .... if split9, *10+8 + #fnum2 =frame # global video frame. if split0, *10+9; if split1, *10+0; if split 2, *10+1 .... if split9, *10+8 + if cam_num>2: + fnum2 *=2 # wideview cams have half the framerate + t = gps_times2[fnum2] # time stamp for the current video frame (same as gps_mark2) + fnum1 = Idfromt(gps_times1,t) # corresponding frame in gps_mark1 + if self.new_distort: + T = self.Ps['T'][Pid[idx]] + P=self.Ps['M_'+str(cam_num)][Pid[idx]] + else: + T = np.eye(4) + P = self.Ps[Pid[idx]] + if frame < start_frame or (final_frame != -1 and frame >= final_frame): + continue + + # car trajectory in current camera frame + local_pts = MapPos(tr1[fnum1:fnum1+290,0:3,3], tr1[fnum1,:,:], cam, T_from_i_to_l) + local_pts[1,:]+=lidar_height # subtract height to get point on ground + # transform according to real-world distortion + local_pts = np.vstack((local_pts, np.ones((1,local_pts.shape[1])))) + local_pts = np.dot(T, local_pts)[0:3,:] + # pick start and end point frame ids + ids = np.where(np.logical_and(gps_times1>t-seconds_ahead*1000000, gps_times10] # make sure in front of camera + depths = lane3d[2,:] + # project into 2d image + (c, J) = cv2.projectPoints(lane3d[0:3,:].transpose(), np.array([0.0,0.0,0.0]), np.array([0.0,0.0,0.0]), cam['KK'], cam['distort']) + # need to define lane id. If necessary split current lane based on lateral distance. + lane3d,lane_id, anchors=self.setLaneIDs(lane3d, center2, sideways_curr) + Lane3d['pts'].append(lane3d) + Lane3d['id'].append(lane_id) + Lane3d['anchors'] = np.r_[Lane3d['anchors'],anchors] + c= warpPoints(P, c[:,0,:].transpose()[0:2,:]) + # scale down to the size of the label mask + labelpix = np.transpose(np.round(c*self.label_scale)) + # scale down to the size of the actual image + imgpix = c*self.img_scale + # find unique indices to be marked in the label mask + #lu = np.ascontiguousarray(labelpix).view(np.dtype((np.void, labelpix.dtype.itemsize * labelpix.shape[1]))) + #_, l_idx = np.unique(lu, return_index=True) + #l_idx = np.sort(l_idx) + labelpix = (np.transpose(labelpix)).astype('i4') + # draw labels on temp masks + if self.visualize: # if need to visualize, make the lines more colorful! + mask_color = l+1 + else: + mask_color=1 + for ii in range(1,imgpix.shape[1]-1): + ip = ii-1 + ic = ii + xp = labelpix[0,ip] + yp = labelpix[1,ip] + xc = labelpix[0,ic] + yc = labelpix[1,ic] + + if np.abs(xp-xc)>1 or np.abs(yp-yc)>1: + x1 = xp + y1 = yp + else: + x1 = xc + y1 = yc + x2 = xc + y2 = yc + if yc>-1 and yc-1 and xc0.5: + #cv2.putText(img, str(int(temp_label[ii,jj]-1)), (reg_label[ii,jj,0],reg_label[ii,jj,1]), cv2.FONT_HERSHEY_PLAIN, 1.0, self.colors[int(temp_label[ii,jj]-1)%len(self.colors)],thickness=1) + + if self.predict_depth: + cv2.line(img, (reg_label[ii,jj,0]*self.img_scale[0],reg_label[ii,jj,1]*self.img_scale[0]), (reg_label[ii,jj,2]*self.img_scale[1], reg_label[ii,jj,3]*self.img_scale[1]), dist2color((reg_label[ii,jj,4]+reg_label[ii,jj,5])/2).tolist(), thickness=2 ) + else: + cv2.line(img, (reg_label[ii,jj,0],reg_label[ii,jj,1]), (reg_label[ii,jj,2], reg_label[ii,jj,3]), self.colors[int(temp_label[ii,jj]-1)%len(self.colors)], thickness=2 ) + cv2.imwrite('/scr/twangcat/lane_detect_results/test2/label_'+str(self.count)+'.png', np.clip(img, 0,255).astype('u1')) + self.count+=1 + # push a batch of data to the data queue + #self.q.put([labels.astype(np.float32),reg_labels,weight_labels,Pid, cam, labels_3d, trajectory_3d]) + label_view = np.transpose(labels, [0,3,1,2]).reshape(batchSize, 1, self.labelh//self.griddim, self.griddim, self.labelw//self.griddim, self.griddim, order='C') + label_grid = np.transpose(label_view,[0,1,3,5,2,4]).reshape(batchSize, self.griddim*self.griddim,self.labelh//self.griddim,self.labelw//self.griddim, order='C') + reg_view = np.transpose(reg_labels, [0,3,1,2]).reshape(batchSize, self.labeld, self.labelh//self.griddim, self.griddim, self.labelw//self.griddim, self.griddim) + reg_grid = np.transpose(reg_view,[0,1,3,5,2,4]).reshape(batchSize, self.labeld*self.griddim*self.griddim,self.labelh//self.griddim,self.labelw//self.griddim) + full_label = np.empty([batchSize, 80, 15, 20], dtype='f4',order='C') + full_label[:,0:16,:,:] = label_grid + full_label[:,16:,:,:] = reg_grid + return full_label + + + + def runLabelling(self, f, frames, Pid): # filename, frame numbers, transformation ids + Pid = Pid.tolist() + print frames + print Pid + cam_num = int(f[-5]) + splitidx = string.index(f,'split_') + split_num = int(f[splitidx+6]) + if split_num==0: + split_num=10 + path, fname = os.path.split(f) + fname = fname[8:] # remove 'split_?' + args = parse_args(path, fname) + prefix = path + '/' + fname + + params = args['params'] + cam = params['cam'][cam_num-1] + self.label_scale = np.array([[float(self.labelw) / cam['width']], [float(self.labelh) / cam['height']]]) + self.img_scale = np.array([[float(self.imwidth) / cam['width']], [float(self.imheight) / cam['height']]]) + if os.path.isfile(args['gps_mark2']): + gps_key1='gps_mark1' + gps_key2='gps_mark2' + postfix_len = 13 + else: + gps_key1='gps' + gps_key2='gps' + postfix_len=8 + + # gps_mark2 + gps_filename2= args[gps_key2] + if not (gps_filename2 in self.gps_values2): # if haven't read this gps file before, cache it in dict. + gps_reader2 = GPSReader(gps_filename2) + self.gps_values2[gps_filename2] = gps_reader2.getNumericData() + gps_data2 = self.gps_values2[gps_filename2] + gps_times2 = utc_from_gps_log_all(gps_data2) + + # gps_mark1 + gps_filename1= args[gps_key1] + if not (gps_filename1 in self.gps_values1): # if haven't read this gps file before, cache it in dict. + gps_reader1 = GPSReader(gps_filename1) + self.gps_values1[gps_filename1] = gps_reader1.getNumericData() + gps_data1 = self.gps_values1[gps_filename1] + tr1 = IMUTransforms(gps_data1) + gps_times1 = utc_from_gps_log_all(gps_data1) + + prefix = gps_filename2[0:-postfix_len] + + #lane_filename = prefix+'_multilane_points_done.npz' + lane_filename = prefix+'_multilane_points_planar_done.npz' + if not (lane_filename in self.lane_values): + self.lane_values[lane_filename] = np.load(lane_filename) + lanes = self.lane_values[lane_filename] # these values are alread pre-computed and saved, now just read it from dictionary + + start_frame = 0 #frames to skip #int(sys.argv[3]) if len(sys.argv) > 3 else 0 + final_frame = -1 #int(sys.argv[4]) if len(sys.argv) > 4 else -1 + return self.runBatch(f, gps_data1, gps_times1, gps_times2, frames, start_frame, final_frame, lanes, tr1, Pid, split_num, cam_num, params) + + + +if __name__ == '__main__': + label_reader = MultilaneLabelReader(400, 0, 3, 640, 480, label_dim = [80,60], readVideo = True,predict_depth=True) + fid = open('/scail/group/deeplearning/driving_data/twangcat/schedules/q50_test_schedule_4-2-14-monterey-17S_a2.avi_96.pickle') + batches = pickle.load(fid) + fid.close() + label_reader.start() + for b in batches: + label_reader.push_batch(b) + diff --git a/src/caffe/py_lane_label_reader/parameters/__init__.py b/src/caffe/py_lane_label_reader/parameters/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/src/caffe/py_lane_label_reader/parameters/q50_3_7_14_params.py b/src/caffe/py_lane_label_reader/parameters/q50_3_7_14_params.py new file mode 100644 index 00000000000..60b4652bf23 --- /dev/null +++ b/src/caffe/py_lane_label_reader/parameters/q50_3_7_14_params.py @@ -0,0 +1,76 @@ +import numpy as np +from transformations import euler_matrix + +# defines calibration parameters for the Q50 setup. + +# IMU orientation -> x forward, y out the left door, z up +# to convert from WGS84, roll, pitch, azimuth to IMU based cartesian coordinate sysem use: + + +# Lidar orientation -> x forward, y out the left door, z up + +# camera orientation -> todo + +##### LIDAR to IMU calibration parameters ##### + +# Note: the translation vector is currently unknown +def GetQ50LidarParams(): + params = { } + params['R_from_i_to_l'] = euler_matrix(-0.005,-0.0081,-0.0375)[0:3,0:3] + params['T_from_l_to_i'] = np.eye(4) + params['T_from_l_to_i'][0:3,0:3] = params['R_from_i_to_l'].transpose() + params['height']=1.85 + return params + +##### LIDAR to Camera Calibration parameters ##### + +def GetQ50CameraParams(): + cam = [{}, {}] + for i in [1, 0]: + cam[i]['R_to_c_from_i'] = np.array([[-1, 0, 0], + [0, 0, -1], + [0, -1, 0]]) + + if i == 0: + cam[i]['R_to_c_from_l_in_camera_frame'] = cam[1]['R_to_c_from_l_in_camera_frame'] + cam[i]['displacement_from_l_to_c_in_lidar_frame'] = cam[1]['displacement_from_l_to_c_in_lidar_frame'] + + # extrinsics parameters for transforming points in right camera frame to this camera + T = np.array([-0.5873763710461054, 0.00012196510170337307, 0.08922401781210791]) + R = np.array([0.9999355343485463, -0.00932576944123699, 0.006477435558612815, 0.009223923954826548, 0.9998360945238545, 0.015578938158992275, -0.006621659456863392, -0.01551818647957998, 0.9998576596268203]) + R = R.reshape((3,3)) + T = T.reshape((3,1)) + E = np.hstack((R.transpose(), np.dot(-R.transpose(),T))) + E = np.vstack((E,np.array([0,0,0,1]))) + cam[i]['E'] = E + cam[i]['fx'] = 2254.76 + cam[i]['fy'] = 2266.30 + cam[i]['cu'] = 655.55 + cam[i]['cv'] = 488.85 + cam[i]['distort'] = np.array([-0.22146000368016028, 0.7987879799679538, -6.542034918087567e-05, 2.8680581938024014e-05, 0.0]) + + elif i == 1: + R_to_c_from_l_in_camera_frame = euler_matrix(0.046,0.0071,0.0065)[0:3,0:3] + cam[i]['R_to_c_from_l_in_camera_frame'] = R_to_c_from_l_in_camera_frame + cam[i]['displacement_from_l_to_c_in_lidar_frame'] = np.array([-0.07, 0.325, 0.16]); + cam[i]['fx'] = 2250.72 + cam[i]['fy'] = 2263.75 + cam[i]['cu'] = 648.95 + cam[i]['cv'] = 450.24 + cam[i]['distort'] = np.array([-0.16879238412882028, 0.11971166628565273, -0.0017457365846050555, 0.0001853749033525837, 0.0]) + cam[i]['E'] = np.eye(4) + + + + cam[i]['KK'] = np.array([[cam[i]['fx'], 0.0, cam[i]['cu']], + [0.0, cam[i]['fy'], cam[i]['cv']], + [0.0, 0.0, 1.0]]) + cam[i]['f'] = (cam[i]['fx'] + cam[i]['fy']) / 2 + + return cam + +def GetQ50Params(): + params = { } + params['lidar'] = GetQ50LidarParams(); + params['cam'] = GetQ50CameraParams(); + return params diff --git a/src/caffe/py_lane_label_reader/parameters/q50_4_3_14_params.py b/src/caffe/py_lane_label_reader/parameters/q50_4_3_14_params.py new file mode 100644 index 00000000000..422aff6745e --- /dev/null +++ b/src/caffe/py_lane_label_reader/parameters/q50_4_3_14_params.py @@ -0,0 +1,147 @@ +import numpy as np +from transformations import euler_matrix + +# defines calibration parameters for the Q50 setup. + +# IMU orientation -> x forward, y out the left door, z up +# to convert from WGS84, roll, pitch, azimuth to IMU based cartesian coordinate sysem use: + + +# Lidar orientation -> x forward, y out the left door, z up + +# camera orientation -> todo + +##### LIDAR to IMU calibration parameters ##### + +def GetQ50LidarParams(): + params = { } + params['R_from_i_to_l'] = euler_matrix(-0.04, -0.0146, -0.0165)[0:3,0:3] + params['T_from_l_to_i'] = np.eye(4) + params['T_from_l_to_i'][0:3,0:3] = params['R_from_i_to_l'].transpose() + params['height'] = 2.0 + return params + +##### LIDAR to Camera Calibration parameters ##### + +def GetQ50CameraParams(): + cam = [{}, {}, {}, {}, {}, {}] + for i in [1, 0, 2, 3, 4, 5]: + cam[i]['R_to_c_from_i'] = np.array([[-1, 0, 0], + [0, 0, -1], + [0, -1, 0]]) + + if i == 0: # left camera + + cam[i]['R_to_c_from_l_in_camera_frame'] = cam[1]['R_to_c_from_l_in_camera_frame'] + cam[i]['displacement_from_l_to_c_in_lidar_frame'] = cam[1]['displacement_from_l_to_c_in_lidar_frame'] + + # extrinsics parameters for transforming points in right camera frame to this camera + T = np.array([-0.5873763710461054, 0.00012196510170337307, 0.08922401781210791]) + R = np.array([0.9999355343485463, -0.00932576944123699, 0.006477435558612815, 0.009223923954826548, 0.9998360945238545, 0.015578938158992275, -0.006621659456863392, -0.01551818647957998, 0.9998576596268203]) + R = R.reshape((3,3)) + T = T.reshape((3,1)) + E = np.hstack((R.transpose(), np.dot(-R.transpose(),T))) + E = np.vstack((E,np.array([0,0,0,1]))) + cam[i]['E'] = E + cam[i]['E_t'] = T + cam[i]['E_R'] = R + cam[i]['width'] = 1280 + cam[i]['height'] = 960 + cam[i]['fx'] = 2254.76 + cam[i]['fy'] = 2266.30 + cam[i]['cu'] = 655.55 + cam[i]['cv'] = 488.85 + cam[i]['distort'] = np.array([-0.22146000368016028, 0.7987879799679538, -6.542034918087567e-05, 2.8680581938024014e-05, 0.0]) + + elif i == 1: # right camera + # R_to_c_from_l_in_camera_frame = euler_matrix(0.044,0.0291,0.0115)[0:3,0:3] + + #R_to_c_from_l_in_camera_frame = euler_matrix(0.042000,0.022000,0.015000)[0:3,0:3] + R_to_c_from_l_in_camera_frame = euler_matrix(0.04116677, 0.02722644, 0.00895531)[0:3,0:3] + cam[i]['R_to_c_from_l_in_camera_frame'] = R_to_c_from_l_in_camera_frame + cam[i]['displacement_from_l_to_c_in_lidar_frame'] = np.array([-0.5,0.31,0.34]); + cam[i]['E'] = np.eye(4) + cam[i]['width'] = 1280 + cam[i]['height'] = 960 + cam[i]['fx'] = 2250.72 + cam[i]['fy'] = 2263.75 + cam[i]['cu'] = 648.95 + cam[i]['cv'] = 450.24 + cam[i]['distort'] = np.array([-0.16879238412882028, 0.11971166628565273, -0.0017457365846050555, 0.0001853749033525837, 0.0]) + + elif i == 2: #wfov front + # FIXME + R_to_c_from_l_in_camera_frame = euler_matrix(-0.005, 0.06, 0.0)[0:3,0:3] + cam[i]['R_to_c_from_l_in_camera_frame'] = R_to_c_from_l_in_camera_frame + # FIXME + cam[i]['displacement_from_l_to_c_in_lidar_frame'] = np.array([-0.53, 0.0, 0.40]) + cam[i]['E'] = np.eye(4) + cam[i]['width'] = 2080 + cam[i]['height'] = 1040 + cam[i]['fx'] = 669.688574 + cam[i]['fy'] = 673.807170 + cam[i]['cu'] = 996.121080 + cam[i]['cv'] = 264.973916*2 + # FIXME TODO + cam[i]['distort'] = np.array([-0.006067, 0.001167, 0.000275, 0.000828, 0.000000]) + + elif i == 3: #wfov left + R_to_c_from_l_in_camera_frame = euler_matrix(1.575, 0.04, -0.01, 'syxz')[0:3,0:3] + cam[i]['R_to_c_from_l_in_camera_frame'] = R_to_c_from_l_in_camera_frame + cam[i]['displacement_from_l_to_c_in_lidar_frame'] = np.array([0.0, -0.762, 0.40]) + cam[i]['E'] = np.eye(4) + cam[i]['width'] = 2080 + cam[i]['height'] = 1040 + cam[i]['fx'] = 669.688574 + cam[i]['fy'] = 673.807170 + cam[i]['cu'] = 996.121080 + cam[i]['cv'] = 264.973916*2 + cam[i]['distort'] = np.array([-0.006067, 0.001167, 0.000275, 0.000828, 0.000000]) + + elif i == 4: #wfov right + R_to_c_from_l_in_camera_frame = euler_matrix(-1.57, 0.06, 0.01, 'syxz')[0:3,0:3] + cam[i]['R_to_c_from_l_in_camera_frame'] = R_to_c_from_l_in_camera_frame + cam[i]['displacement_from_l_to_c_in_lidar_frame'] = np.array([0.0, 0.67, 0.40]) + cam[i]['E'] = np.eye(4) + cam[i]['width'] = 2080 + cam[i]['height'] = 1040 + cam[i]['fx'] = 669.688574 + cam[i]['fy'] = 673.807170 + cam[i]['cu'] = 996.121080 + cam[i]['cv'] = 264.973916*2 + cam[i]['distort'] = np.array([-0.006067, 0.001167, 0.000275, 0.000828, 0.000000]) + + elif i == 5: #wfov back + R_to_c_from_l_in_camera_frame = euler_matrix(0.005, 3.14, 0.005)[0:3,0:3] + cam[i]['R_to_c_from_l_in_camera_frame'] = R_to_c_from_l_in_camera_frame + cam[i]['displacement_from_l_to_c_in_lidar_frame'] = np.array([0.50, 0.0, 0.40]) + cam[i]['E'] = np.eye(4) + cam[i]['width'] = 2080 + cam[i]['height'] = 1040 + cam[i]['fx'] = 669.688574 + cam[i]['fy'] = 673.807170 + cam[i]['cu'] = 996.121080 + cam[i]['cv'] = 264.973916*2 + cam[i]['distort'] = np.array([-0.006067, 0.001167, 0.000275, 0.000828, 0.000000]) + + cam[i]['KK'] = np.array([[cam[i]['fx'], 0.0, cam[i]['cu']], + [0.0, cam[i]['fy'], cam[i]['cv']], + [0.0, 0.0, 1.0]]) + cam[i]['f'] = (cam[i]['fx'] + cam[i]['fy']) / 2 + + return cam + + +def GetQ50RadarParams(): + params = { } + params['R_from_r_to_l'] = euler_matrix(0, 0, -.009)[0:3,0:3] + params['T_from_r_to_l'] = [3.17, 0.4, -1.64] + + return params + +def GetQ50Params(): + params = { } + params['lidar'] = GetQ50LidarParams(); + params['cam'] = GetQ50CameraParams(); + params['radar'] = GetQ50RadarParams(); + return params diff --git a/src/caffe/py_lane_label_reader/parameters/transformations.py b/src/caffe/py_lane_label_reader/parameters/transformations.py new file mode 100644 index 00000000000..c31b66050d8 --- /dev/null +++ b/src/caffe/py_lane_label_reader/parameters/transformations.py @@ -0,0 +1,1907 @@ +# -*- coding: utf-8 -*- +# transformations.py + +# Copyright (c) 2006-2013, Christoph Gohlke +# Copyright (c) 2006-2013, The Regents of the University of California +# Produced at the Laboratory for Fluorescence Dynamics +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the copyright holders nor the names of any +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Homogeneous Transformation Matrices and Quaternions. + +A library for calculating 4x4 matrices for translating, rotating, reflecting, +scaling, shearing, projecting, orthogonalizing, and superimposing arrays of +3D homogeneous coordinates as well as for converting between rotation matrices, +Euler angles, and quaternions. Also includes an Arcball control object and +functions to decompose transformation matrices. + +:Author: + `Christoph Gohlke `_ + +:Organization: + Laboratory for Fluorescence Dynamics, University of California, Irvine + +:Version: 2013.04.16 + +Requirements +------------ +* `CPython 2.7, 3.2 or 3.3 `_ +* `Numpy 1.7 `_ +* `Transformations.c 2013.01.18 `_ + (recommended for speedup of some functions) + +Notes +----- +The API is not stable yet and is expected to change between revisions. + +This Python code is not optimized for speed. Refer to the transformations.c +module for a faster implementation of some functions. + +Documentation in HTML format can be generated with epydoc. + +Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using +numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using +numpy.dot(M, v) for shape (4, \*) column vectors, respectively +numpy.dot(v, M.T) for shape (\*, 4) row vectors ("array of points"). + +This module follows the "column vectors on the right" and "row major storage" +(C contiguous) conventions. The translation components are in the right column +of the transformation matrix, i.e. M[:3, 3]. +The transpose of the transformation matrices may have to be used to interface +with other graphics systems, e.g. with OpenGL's glMultMatrixd(). See also [16]. + +Calculations are carried out with numpy.float64 precision. + +Vector, point, quaternion, and matrix function arguments are expected to be +"array like", i.e. tuple, list, or numpy arrays. + +Return types are numpy arrays unless specified otherwise. + +Angles are in radians unless specified otherwise. + +Quaternions w+ix+jy+kz are represented as [w, x, y, z]. + +A triple of Euler angles can be applied/interpreted in 24 ways, which can +be specified using a 4 character string or encoded 4-tuple: + + *Axes 4-string*: e.g. 'sxyz' or 'ryxy' + + - first character : rotations are applied to 's'tatic or 'r'otating frame + - remaining characters : successive rotation axis 'x', 'y', or 'z' + + *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) + + - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. + - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed + by 'z', or 'z' is followed by 'x'. Otherwise odd (1). + - repetition : first and last axis are same (1) or different (0). + - frame : rotations are applied to static (0) or rotating (1) frame. + +References +---------- +(1) Matrices and transformations. Ronald Goldman. + In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. +(2) More matrices and transformations: shear and pseudo-perspective. + Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. +(3) Decomposing a matrix into simple transformations. Spencer Thomas. + In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. +(4) Recovering the data from the transformation matrix. Ronald Goldman. + In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. +(5) Euler angle conversion. Ken Shoemake. + In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. +(6) Arcball rotation control. Ken Shoemake. + In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. +(7) Representing attitude: Euler angles, unit quaternions, and rotation + vectors. James Diebel. 2006. +(8) A discussion of the solution for the best rotation to relate two sets + of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. +(9) Closed-form solution of absolute orientation using unit quaternions. + BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642. +(10) Quaternions. Ken Shoemake. + http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf +(11) From quaternion to matrix and back. JMP van Waveren. 2005. + http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm +(12) Uniform random rotations. Ken Shoemake. + In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. +(13) Quaternion in molecular modeling. CFF Karney. + J Mol Graph Mod, 25(5):595-604 +(14) New method for extracting the quaternion from a rotation matrix. + Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087. +(15) Multiple View Geometry in Computer Vision. Hartley and Zissermann. + Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130. +(16) Column Vectors vs. Row Vectors. + http://steve.hollasch.net/cgindex/math/matrix/column-vec.html + +Examples +-------- +>>> alpha, beta, gamma = 0.123, -1.234, 2.345 +>>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] +>>> I = identity_matrix() +>>> Rx = rotation_matrix(alpha, xaxis) +>>> Ry = rotation_matrix(beta, yaxis) +>>> Rz = rotation_matrix(gamma, zaxis) +>>> R = concatenate_matrices(Rx, Ry, Rz) +>>> euler = euler_from_matrix(R, 'rxyz') +>>> numpy.allclose([alpha, beta, gamma], euler) +True +>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') +>>> is_same_transform(R, Re) +True +>>> al, be, ga = euler_from_matrix(Re, 'rxyz') +>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) +True +>>> qx = quaternion_about_axis(alpha, xaxis) +>>> qy = quaternion_about_axis(beta, yaxis) +>>> qz = quaternion_about_axis(gamma, zaxis) +>>> q = quaternion_multiply(qx, qy) +>>> q = quaternion_multiply(q, qz) +>>> Rq = quaternion_matrix(q) +>>> is_same_transform(R, Rq) +True +>>> S = scale_matrix(1.23, origin) +>>> T = translation_matrix([1, 2, 3]) +>>> Z = shear_matrix(beta, xaxis, origin, zaxis) +>>> R = random_rotation_matrix(numpy.random.rand(3)) +>>> M = concatenate_matrices(T, R, Z, S) +>>> scale, shear, angles, trans, persp = decompose_matrix(M) +>>> numpy.allclose(scale, 1.23) +True +>>> numpy.allclose(trans, [1, 2, 3]) +True +>>> numpy.allclose(shear, [0, math.tan(beta), 0]) +True +>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) +True +>>> M1 = compose_matrix(scale, shear, angles, trans, persp) +>>> is_same_transform(M, M1) +True +>>> v0, v1 = random_vector(3), random_vector(3) +>>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1)) +>>> v2 = numpy.dot(v0, M[:3,:3].T) +>>> numpy.allclose(unit_vector(v1), unit_vector(v2)) +True + +""" + +from __future__ import division, print_function + +import math + +import numpy + +__version__ = '2013.04.16' +__docformat__ = 'restructuredtext en' +__all__ = [] + + +def identity_matrix(): + """Return 4x4 identity/unit matrix. + + >>> I = identity_matrix() + >>> numpy.allclose(I, numpy.dot(I, I)) + True + >>> numpy.sum(I), numpy.trace(I) + (4.0, 4.0) + >>> numpy.allclose(I, numpy.identity(4)) + True + + """ + return numpy.identity(4) + + +def translation_matrix(direction): + """Return matrix to translate by direction vector. + + >>> v = numpy.random.random(3) - 0.5 + >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) + True + + """ + M = numpy.identity(4) + M[:3, 3] = direction[:3] + return M + + +def translation_from_matrix(matrix): + """Return translation vector from translation matrix. + + >>> v0 = numpy.random.random(3) - 0.5 + >>> v1 = translation_from_matrix(translation_matrix(v0)) + >>> numpy.allclose(v0, v1) + True + + """ + return numpy.array(matrix, copy=False)[:3, 3].copy() + + +def reflection_matrix(point, normal): + """Return matrix to mirror at plane defined by point and normal vector. + + >>> v0 = numpy.random.random(4) - 0.5 + >>> v0[3] = 1. + >>> v1 = numpy.random.random(3) - 0.5 + >>> R = reflection_matrix(v0, v1) + >>> numpy.allclose(2, numpy.trace(R)) + True + >>> numpy.allclose(v0, numpy.dot(R, v0)) + True + >>> v2 = v0.copy() + >>> v2[:3] += v1 + >>> v3 = v0.copy() + >>> v2[:3] -= v1 + >>> numpy.allclose(v2, numpy.dot(R, v3)) + True + + """ + normal = unit_vector(normal[:3]) + M = numpy.identity(4) + M[:3, :3] -= 2.0 * numpy.outer(normal, normal) + M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal + return M + + +def reflection_from_matrix(matrix): + """Return mirror plane point and normal vector from reflection matrix. + + >>> v0 = numpy.random.random(3) - 0.5 + >>> v1 = numpy.random.random(3) - 0.5 + >>> M0 = reflection_matrix(v0, v1) + >>> point, normal = reflection_from_matrix(M0) + >>> M1 = reflection_matrix(point, normal) + >>> is_same_transform(M0, M1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + # normal: unit eigenvector corresponding to eigenvalue -1 + w, V = numpy.linalg.eig(M[:3, :3]) + i = numpy.where(abs(numpy.real(w) + 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue -1") + normal = numpy.real(V[:, i[0]]).squeeze() + # point: any unit eigenvector corresponding to eigenvalue 1 + w, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + return point, normal + + +def rotation_matrix(angle, direction, point=None): + """Return matrix to rotate about axis defined by point and direction. + + >>> R = rotation_matrix(math.pi/2, [0, 0, 1], [1, 0, 0]) + >>> numpy.allclose(numpy.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1]) + True + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) + >>> is_same_transform(R0, R1) + True + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(-angle, -direc, point) + >>> is_same_transform(R0, R1) + True + >>> I = numpy.identity(4, numpy.float64) + >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) + True + >>> numpy.allclose(2, numpy.trace(rotation_matrix(math.pi/2, + ... direc, point))) + True + + """ + sina = math.sin(angle) + cosa = math.cos(angle) + direction = unit_vector(direction[:3]) + # rotation matrix around unit vector + R = numpy.diag([cosa, cosa, cosa]) + R += numpy.outer(direction, direction) * (1.0 - cosa) + direction *= sina + R += numpy.array([[ 0.0, -direction[2], direction[1]], + [ direction[2], 0.0, -direction[0]], + [-direction[1], direction[0], 0.0]]) + M = numpy.identity(4) + M[:3, :3] = R + if point is not None: + # rotation not around origin + point = numpy.array(point[:3], dtype=numpy.float64, copy=False) + M[:3, 3] = point - numpy.dot(R, point) + return M + + +def rotation_from_matrix(matrix): + """Return rotation angle and axis from rotation matrix. + + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> angle, direc, point = rotation_from_matrix(R0) + >>> R1 = rotation_matrix(angle, direc, point) + >>> is_same_transform(R0, R1) + True + + """ + R = numpy.array(matrix, dtype=numpy.float64, copy=False) + R33 = R[:3, :3] + # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 + w, W = numpy.linalg.eig(R33.T) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + direction = numpy.real(W[:, i[-1]]).squeeze() + # point: unit eigenvector of R33 corresponding to eigenvalue of 1 + w, Q = numpy.linalg.eig(R) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + point = numpy.real(Q[:, i[-1]]).squeeze() + point /= point[3] + # rotation angle depending on direction + cosa = (numpy.trace(R33) - 1.0) / 2.0 + if abs(direction[2]) > 1e-8: + sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] + elif abs(direction[1]) > 1e-8: + sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] + else: + sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] + angle = math.atan2(sina, cosa) + return angle, direction, point + + +def scale_matrix(factor, origin=None, direction=None): + """Return matrix to scale by factor around origin in direction. + + Use factor -1 for point symmetry. + + >>> v = (numpy.random.rand(4, 5) - 0.5) * 20 + >>> v[3] = 1 + >>> S = scale_matrix(-1.234) + >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) + True + >>> factor = random.random() * 10 - 5 + >>> origin = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> S = scale_matrix(factor, origin) + >>> S = scale_matrix(factor, origin, direct) + + """ + if direction is None: + # uniform scaling + M = numpy.diag([factor, factor, factor, 1.0]) + if origin is not None: + M[:3, 3] = origin[:3] + M[:3, 3] *= 1.0 - factor + else: + # nonuniform scaling + direction = unit_vector(direction[:3]) + factor = 1.0 - factor + M = numpy.identity(4) + M[:3, :3] -= factor * numpy.outer(direction, direction) + if origin is not None: + M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction + return M + + +def scale_from_matrix(matrix): + """Return scaling factor, origin and direction from scaling matrix. + + >>> factor = random.random() * 10 - 5 + >>> origin = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> S0 = scale_matrix(factor, origin) + >>> factor, origin, direction = scale_from_matrix(S0) + >>> S1 = scale_matrix(factor, origin, direction) + >>> is_same_transform(S0, S1) + True + >>> S0 = scale_matrix(factor, origin, direct) + >>> factor, origin, direction = scale_from_matrix(S0) + >>> S1 = scale_matrix(factor, origin, direction) + >>> is_same_transform(S0, S1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + factor = numpy.trace(M33) - 2.0 + try: + # direction: unit eigenvector corresponding to eigenvalue factor + w, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(w) - factor) < 1e-8)[0][0] + direction = numpy.real(V[:, i]).squeeze() + direction /= vector_norm(direction) + except IndexError: + # uniform scaling + factor = (factor + 2.0) / 3.0 + direction = None + # origin: any eigenvector corresponding to eigenvalue 1 + w, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 1") + origin = numpy.real(V[:, i[-1]]).squeeze() + origin /= origin[3] + return factor, origin, direction + + +def projection_matrix(point, normal, direction=None, + perspective=None, pseudo=False): + """Return matrix to project onto plane defined by point and normal. + + Using either perspective point, projection direction, or none of both. + + If pseudo is True, perspective projections will preserve relative depth + such that Perspective = dot(Orthogonal, PseudoPerspective). + + >>> P = projection_matrix([0, 0, 0], [1, 0, 0]) + >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) + True + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(3) - 0.5 + >>> P0 = projection_matrix(point, normal) + >>> P1 = projection_matrix(point, normal, direction=direct) + >>> P2 = projection_matrix(point, normal, perspective=persp) + >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) + >>> is_same_transform(P2, numpy.dot(P0, P3)) + True + >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0]) + >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20 + >>> v0[3] = 1 + >>> v1 = numpy.dot(P, v0) + >>> numpy.allclose(v1[1], v0[1]) + True + >>> numpy.allclose(v1[0], 3-v1[1]) + True + + """ + M = numpy.identity(4) + point = numpy.array(point[:3], dtype=numpy.float64, copy=False) + normal = unit_vector(normal[:3]) + if perspective is not None: + # perspective projection + perspective = numpy.array(perspective[:3], dtype=numpy.float64, + copy=False) + M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) + M[:3, :3] -= numpy.outer(perspective, normal) + if pseudo: + # preserve relative depth + M[:3, :3] -= numpy.outer(normal, normal) + M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) + else: + M[:3, 3] = numpy.dot(point, normal) * perspective + M[3, :3] = -normal + M[3, 3] = numpy.dot(perspective, normal) + elif direction is not None: + # parallel projection + direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) + scale = numpy.dot(direction, normal) + M[:3, :3] -= numpy.outer(direction, normal) / scale + M[:3, 3] = direction * (numpy.dot(point, normal) / scale) + else: + # orthogonal projection + M[:3, :3] -= numpy.outer(normal, normal) + M[:3, 3] = numpy.dot(point, normal) * normal + return M + + +def projection_from_matrix(matrix, pseudo=False): + """Return projection plane and perspective point from projection matrix. + + Return values are same as arguments for projection_matrix function: + point, normal, direction, perspective, and pseudo. + + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(3) - 0.5 + >>> P0 = projection_matrix(point, normal) + >>> result = projection_from_matrix(P0) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, direct) + >>> result = projection_from_matrix(P0) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) + >>> result = projection_from_matrix(P0, pseudo=False) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) + >>> result = projection_from_matrix(P0, pseudo=True) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + w, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not pseudo and len(i): + # point: any eigenvector corresponding to eigenvalue 1 + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + # direction: unit eigenvector corresponding to eigenvalue 0 + w, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 0") + direction = numpy.real(V[:, i[0]]).squeeze() + direction /= vector_norm(direction) + # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 + w, V = numpy.linalg.eig(M33.T) + i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] + if len(i): + # parallel projection + normal = numpy.real(V[:, i[0]]).squeeze() + normal /= vector_norm(normal) + return point, normal, direction, None, False + else: + # orthogonal projection, where normal equals direction vector + return point, direction, None, None, False + else: + # perspective projection + i = numpy.where(abs(numpy.real(w)) > 1e-8)[0] + if not len(i): + raise ValueError( + "no eigenvector not corresponding to eigenvalue 0") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + normal = - M[3, :3] + perspective = M[:3, 3] / numpy.dot(point[:3], normal) + if pseudo: + perspective -= normal + return point, normal, None, perspective, pseudo + + +def clip_matrix(left, right, bottom, top, near, far, perspective=False): + """Return matrix to obtain normalized device coordinates from frustum. + + The frustum bounds are axis-aligned along x (left, right), + y (bottom, top) and z (near, far). + + Normalized device coordinates are in range [-1, 1] if coordinates are + inside the frustum. + + If perspective is True the frustum is a truncated pyramid with the + perspective point at origin and direction along z axis, otherwise an + orthographic canonical view volume (a box). + + Homogeneous coordinates transformed by the perspective clip matrix + need to be dehomogenized (divided by w coordinate). + + >>> frustum = numpy.random.rand(6) + >>> frustum[1] += frustum[0] + >>> frustum[3] += frustum[2] + >>> frustum[5] += frustum[4] + >>> M = clip_matrix(perspective=False, *frustum) + >>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) + array([-1., -1., -1., 1.]) + >>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1]) + array([ 1., 1., 1., 1.]) + >>> M = clip_matrix(perspective=True, *frustum) + >>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) + >>> v / v[3] + array([-1., -1., -1., 1.]) + >>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1]) + >>> v / v[3] + array([ 1., 1., -1., 1.]) + + """ + if left >= right or bottom >= top or near >= far: + raise ValueError("invalid frustum") + if perspective: + if near <= _EPS: + raise ValueError("invalid frustum: near <= 0") + t = 2.0 * near + M = [[t/(left-right), 0.0, (right+left)/(right-left), 0.0], + [0.0, t/(bottom-top), (top+bottom)/(top-bottom), 0.0], + [0.0, 0.0, (far+near)/(near-far), t*far/(far-near)], + [0.0, 0.0, -1.0, 0.0]] + else: + M = [[2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)], + [0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)], + [0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)], + [0.0, 0.0, 0.0, 1.0]] + return numpy.array(M) + + +def shear_matrix(angle, direction, point, normal): + """Return matrix to shear by angle along direction vector on shear plane. + + The shear plane is defined by a point and normal vector. The direction + vector must be orthogonal to the plane's normal vector. + + A point P is transformed by the shear matrix into P" such that + the vector P-P" is parallel to the direction vector and its extent is + given by the angle of P-P'-P", where P' is the orthogonal projection + of P onto the shear plane. + + >>> angle = (random.random() - 0.5) * 4*math.pi + >>> direct = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.cross(direct, numpy.random.random(3)) + >>> S = shear_matrix(angle, direct, point, normal) + >>> numpy.allclose(1, numpy.linalg.det(S)) + True + + """ + normal = unit_vector(normal[:3]) + direction = unit_vector(direction[:3]) + if abs(numpy.dot(normal, direction)) > 1e-6: + raise ValueError("direction and normal vectors are not orthogonal") + angle = math.tan(angle) + M = numpy.identity(4) + M[:3, :3] += angle * numpy.outer(direction, normal) + M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction + return M + + +def shear_from_matrix(matrix): + """Return shear angle, direction and plane from shear matrix. + + >>> angle = (random.random() - 0.5) * 4*math.pi + >>> direct = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.cross(direct, numpy.random.random(3)) + >>> S0 = shear_matrix(angle, direct, point, normal) + >>> angle, direct, point, normal = shear_from_matrix(S0) + >>> S1 = shear_matrix(angle, direct, point, normal) + >>> is_same_transform(S0, S1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + # normal: cross independent eigenvectors corresponding to the eigenvalue 1 + w, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-4)[0] + if len(i) < 2: + raise ValueError("no two linear independent eigenvectors found %s" % w) + V = numpy.real(V[:, i]).squeeze().T + lenorm = -1.0 + for i0, i1 in ((0, 1), (0, 2), (1, 2)): + n = numpy.cross(V[i0], V[i1]) + w = vector_norm(n) + if w > lenorm: + lenorm = w + normal = n + normal /= lenorm + # direction and angle + direction = numpy.dot(M33 - numpy.identity(3), normal) + angle = vector_norm(direction) + direction /= angle + angle = math.atan(angle) + # point: eigenvector corresponding to eigenvalue 1 + w, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 1") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + return angle, direction, point, normal + + +def decompose_matrix(matrix): + """Return sequence of transformations from transformation matrix. + + matrix : array_like + Non-degenerative homogeneous transformation matrix + + Return tuple of: + scale : vector of 3 scaling factors + shear : list of shear factors for x-y, x-z, y-z axes + angles : list of Euler angles about static x, y, z axes + translate : translation vector along x, y, z axes + perspective : perspective partition of matrix + + Raise ValueError if matrix is of wrong type or degenerative. + + >>> T0 = translation_matrix([1, 2, 3]) + >>> scale, shear, angles, trans, persp = decompose_matrix(T0) + >>> T1 = translation_matrix(trans) + >>> numpy.allclose(T0, T1) + True + >>> S = scale_matrix(0.123) + >>> scale, shear, angles, trans, persp = decompose_matrix(S) + >>> scale[0] + 0.123 + >>> R0 = euler_matrix(1, 2, 3) + >>> scale, shear, angles, trans, persp = decompose_matrix(R0) + >>> R1 = euler_matrix(*angles) + >>> numpy.allclose(R0, R1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=True).T + if abs(M[3, 3]) < _EPS: + raise ValueError("M[3, 3] is zero") + M /= M[3, 3] + P = M.copy() + P[:, 3] = 0.0, 0.0, 0.0, 1.0 + if not numpy.linalg.det(P): + raise ValueError("matrix is singular") + + scale = numpy.zeros((3, )) + shear = [0.0, 0.0, 0.0] + angles = [0.0, 0.0, 0.0] + + if any(abs(M[:3, 3]) > _EPS): + perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) + M[:, 3] = 0.0, 0.0, 0.0, 1.0 + else: + perspective = numpy.array([0.0, 0.0, 0.0, 1.0]) + + translate = M[3, :3].copy() + M[3, :3] = 0.0 + + row = M[:3, :3].copy() + scale[0] = vector_norm(row[0]) + row[0] /= scale[0] + shear[0] = numpy.dot(row[0], row[1]) + row[1] -= row[0] * shear[0] + scale[1] = vector_norm(row[1]) + row[1] /= scale[1] + shear[0] /= scale[1] + shear[1] = numpy.dot(row[0], row[2]) + row[2] -= row[0] * shear[1] + shear[2] = numpy.dot(row[1], row[2]) + row[2] -= row[1] * shear[2] + scale[2] = vector_norm(row[2]) + row[2] /= scale[2] + shear[1:] /= scale[2] + + if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: + numpy.negative(scale, scale) + numpy.negative(row, row) + + angles[1] = math.asin(-row[0, 2]) + if math.cos(angles[1]): + angles[0] = math.atan2(row[1, 2], row[2, 2]) + angles[2] = math.atan2(row[0, 1], row[0, 0]) + else: + #angles[0] = math.atan2(row[1, 0], row[1, 1]) + angles[0] = math.atan2(-row[2, 1], row[1, 1]) + angles[2] = 0.0 + + return scale, shear, angles, translate, perspective + + +def compose_matrix(scale=None, shear=None, angles=None, translate=None, + perspective=None): + """Return transformation matrix from sequence of transformations. + + This is the inverse of the decompose_matrix function. + + Sequence of transformations: + scale : vector of 3 scaling factors + shear : list of shear factors for x-y, x-z, y-z axes + angles : list of Euler angles about static x, y, z axes + translate : translation vector along x, y, z axes + perspective : perspective partition of matrix + + >>> scale = numpy.random.random(3) - 0.5 + >>> shear = numpy.random.random(3) - 0.5 + >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) + >>> trans = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(4) - 0.5 + >>> M0 = compose_matrix(scale, shear, angles, trans, persp) + >>> result = decompose_matrix(M0) + >>> M1 = compose_matrix(*result) + >>> is_same_transform(M0, M1) + True + + """ + M = numpy.identity(4) + if perspective is not None: + P = numpy.identity(4) + P[3, :] = perspective[:4] + M = numpy.dot(M, P) + if translate is not None: + T = numpy.identity(4) + T[:3, 3] = translate[:3] + M = numpy.dot(M, T) + if angles is not None: + R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') + M = numpy.dot(M, R) + if shear is not None: + Z = numpy.identity(4) + Z[1, 2] = shear[2] + Z[0, 2] = shear[1] + Z[0, 1] = shear[0] + M = numpy.dot(M, Z) + if scale is not None: + S = numpy.identity(4) + S[0, 0] = scale[0] + S[1, 1] = scale[1] + S[2, 2] = scale[2] + M = numpy.dot(M, S) + M /= M[3, 3] + return M + + +def orthogonalization_matrix(lengths, angles): + """Return orthogonalization matrix for crystallographic cell coordinates. + + Angles are expected in degrees. + + The de-orthogonalization matrix is the inverse. + + >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90]) + >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) + True + >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) + >>> numpy.allclose(numpy.sum(O), 43.063229) + True + + """ + a, b, c = lengths + angles = numpy.radians(angles) + sina, sinb, _ = numpy.sin(angles) + cosa, cosb, cosg = numpy.cos(angles) + co = (cosa * cosb - cosg) / (sina * sinb) + return numpy.array([ + [ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0], + [-a*sinb*co, b*sina, 0.0, 0.0], + [ a*cosb, b*cosa, c, 0.0], + [ 0.0, 0.0, 0.0, 1.0]]) + + +def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): + """Return affine transform matrix to register two point sets. + + v0 and v1 are shape (ndims, \*) arrays of at least ndims non-homogeneous + coordinates, where ndims is the dimensionality of the coordinate space. + + If shear is False, a similarity transformation matrix is returned. + If also scale is False, a rigid/Euclidean transformation matrix + is returned. + + By default the algorithm by Hartley and Zissermann [15] is used. + If usesvd is True, similarity and Euclidean transformation matrices + are calculated by minimizing the weighted sum of squared deviations + (RMSD) according to the algorithm by Kabsch [8]. + Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9] + is used, which is slower when using this Python implementation. + + The returned matrix performs rotation, translation and uniform scaling + (if specified). + + >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]] + >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]] + >>> affine_matrix_from_points(v0, v1) + array([[ 0.14549, 0.00062, 675.50008], + [ 0.00048, 0.14094, 53.24971], + [ 0. , 0. , 1. ]]) + >>> T = translation_matrix(numpy.random.random(3)-0.5) + >>> R = random_rotation_matrix(numpy.random.random(3)) + >>> S = scale_matrix(random.random()) + >>> M = concatenate_matrices(T, R, S) + >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 + >>> v0[3] = 1 + >>> v1 = numpy.dot(M, v0) + >>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1) + >>> M = affine_matrix_from_points(v0[:3], v1[:3]) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + + More examples in superimposition_matrix() + + """ + v0 = numpy.array(v0, dtype=numpy.float64, copy=True) + v1 = numpy.array(v1, dtype=numpy.float64, copy=True) + + ndims = v0.shape[0] + if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape: + raise ValueError("input arrays are of wrong shape or type") + + # move centroids to origin + t0 = -numpy.mean(v0, axis=1) + M0 = numpy.identity(ndims+1) + M0[:ndims, ndims] = t0 + v0 += t0.reshape(ndims, 1) + t1 = -numpy.mean(v1, axis=1) + M1 = numpy.identity(ndims+1) + M1[:ndims, ndims] = t1 + v1 += t1.reshape(ndims, 1) + + if shear: + # Affine transformation + A = numpy.concatenate((v0, v1), axis=0) + u, s, vh = numpy.linalg.svd(A.T) + vh = vh[:ndims].T + B = vh[:ndims] + C = vh[ndims:2*ndims] + t = numpy.dot(C, numpy.linalg.pinv(B)) + t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1) + M = numpy.vstack((t, ((0.0,)*ndims) + (1.0,))) + elif usesvd or ndims != 3: + # Rigid transformation via SVD of covariance matrix + u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) + # rotation matrix from SVD orthonormal bases + R = numpy.dot(u, vh) + if numpy.linalg.det(R) < 0.0: + # R does not constitute right handed system + R -= numpy.outer(u[:, ndims-1], vh[ndims-1, :]*2.0) + s[-1] *= -1.0 + # homogeneous transformation matrix + M = numpy.identity(ndims+1) + M[:ndims, :ndims] = R + else: + # Rigid transformation matrix via quaternion + # compute symmetric matrix N + xx, yy, zz = numpy.sum(v0 * v1, axis=1) + xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) + xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) + N = [[xx+yy+zz, 0.0, 0.0, 0.0], + [yz-zy, xx-yy-zz, 0.0, 0.0], + [zx-xz, xy+yx, yy-xx-zz, 0.0], + [xy-yx, zx+xz, yz+zy, zz-xx-yy]] + # quaternion: eigenvector corresponding to most positive eigenvalue + w, V = numpy.linalg.eigh(N) + q = V[:, numpy.argmax(w)] + q /= vector_norm(q) # unit quaternion + # homogeneous transformation matrix + M = quaternion_matrix(q) + + if scale and not shear: + # Affine transformation; scale is ratio of RMS deviations from centroid + v0 *= v0 + v1 *= v1 + M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) + + # move centroids back + M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0)) + M /= M[ndims, ndims] + return M + + +def superimposition_matrix(v0, v1, scale=False, usesvd=True): + """Return matrix to transform given 3D point set into second point set. + + v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 points. + + The parameters scale and usesvd are explained in the more general + affine_matrix_from_points function. + + The returned matrix is a similarity or Euclidean transformation matrix. + This function has a fast C implementation in transformations.c. + + >>> v0 = numpy.random.rand(3, 10) + >>> M = superimposition_matrix(v0, v0) + >>> numpy.allclose(M, numpy.identity(4)) + True + >>> R = random_rotation_matrix(numpy.random.random(3)) + >>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]] + >>> v1 = numpy.dot(R, v0) + >>> M = superimposition_matrix(v0, v1) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 + >>> v0[3] = 1 + >>> v1 = numpy.dot(R, v0) + >>> M = superimposition_matrix(v0, v1) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> S = scale_matrix(random.random()) + >>> T = translation_matrix(numpy.random.random(3)-0.5) + >>> M = concatenate_matrices(T, R, S) + >>> v1 = numpy.dot(M, v0) + >>> v0[:3] += numpy.random.normal(0, 1e-9, 300).reshape(3, -1) + >>> M = superimposition_matrix(v0, v1, scale=True) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> v = numpy.empty((4, 100, 3)) + >>> v[:, :, 0] = v0 + >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) + >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) + True + + """ + v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] + v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] + return affine_matrix_from_points(v0, v1, shear=False, + scale=scale, usesvd=usesvd) + + +def euler_matrix(ai, aj, ak, axes='sxyz'): + """Return homogeneous rotation matrix from Euler angles and axis sequence. + + ai, aj, ak : Euler's roll, pitch and yaw angles + axes : One of 24 axis sequences as string or encoded tuple + + >>> R = euler_matrix(1, 2, 3, 'syxz') + >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) + True + >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) + >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) + True + >>> ai, aj, ak = (4*math.pi) * (numpy.random.random(3) - 0.5) + >>> for axes in _AXES2TUPLE.keys(): + ... R = euler_matrix(ai, aj, ak, axes) + >>> for axes in _TUPLE2AXES.keys(): + ... R = euler_matrix(ai, aj, ak, axes) + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] + except (AttributeError, KeyError): + _TUPLE2AXES[axes] # validation + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + if frame: + ai, ak = ak, ai + if parity: + ai, aj, ak = -ai, -aj, -ak + + si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) + ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) + cc, cs = ci*ck, ci*sk + sc, ss = si*ck, si*sk + + M = numpy.identity(4) + if repetition: + M[i, i] = cj + M[i, j] = sj*si + M[i, k] = sj*ci + M[j, i] = sj*sk + M[j, j] = -cj*ss+cc + M[j, k] = -cj*cs-sc + M[k, i] = -sj*ck + M[k, j] = cj*sc+cs + M[k, k] = cj*cc-ss + else: + M[i, i] = cj*ck + M[i, j] = sj*sc-cs + M[i, k] = sj*cc+ss + M[j, i] = cj*sk + M[j, j] = sj*ss+cc + M[j, k] = sj*cs-sc + M[k, i] = -sj + M[k, j] = cj*si + M[k, k] = cj*ci + return M + + +def euler_from_matrix(matrix, axes='sxyz'): + """Return Euler angles from rotation matrix for specified axis sequence. + + axes : One of 24 axis sequences as string or encoded tuple + + Note that many Euler angle triplets can describe one matrix. + + >>> R0 = euler_matrix(1, 2, 3, 'syxz') + >>> al, be, ga = euler_from_matrix(R0, 'syxz') + >>> R1 = euler_matrix(al, be, ga, 'syxz') + >>> numpy.allclose(R0, R1) + True + >>> angles = (4*math.pi) * (numpy.random.random(3) - 0.5) + >>> for axes in _AXES2TUPLE.keys(): + ... R0 = euler_matrix(axes=axes, *angles) + ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) + ... if not numpy.allclose(R0, R1): print(axes, "failed") + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] + except (AttributeError, KeyError): + _TUPLE2AXES[axes] # validation + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] + if repetition: + sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) + if sy > _EPS: + ax = math.atan2( M[i, j], M[i, k]) + ay = math.atan2( sy, M[i, i]) + az = math.atan2( M[j, i], -M[k, i]) + else: + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2( sy, M[i, i]) + az = 0.0 + else: + cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) + if cy > _EPS: + ax = math.atan2( M[k, j], M[k, k]) + ay = math.atan2(-M[k, i], cy) + az = math.atan2( M[j, i], M[i, i]) + else: + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2(-M[k, i], cy) + az = 0.0 + + if parity: + ax, ay, az = -ax, -ay, -az + if frame: + ax, az = az, ax + return ax, ay, az + + +def euler_from_quaternion(quaternion, axes='sxyz'): + """Return Euler angles from quaternion for specified axis sequence. + + >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0]) + >>> numpy.allclose(angles, [0.123, 0, 0]) + True + + """ + return euler_from_matrix(quaternion_matrix(quaternion), axes) + + +def quaternion_from_euler(ai, aj, ak, axes='sxyz'): + """Return quaternion from Euler angles and axis sequence. + + ai, aj, ak : Euler's roll, pitch and yaw angles + axes : One of 24 axis sequences as string or encoded tuple + + >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') + >>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435]) + True + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] + except (AttributeError, KeyError): + _TUPLE2AXES[axes] # validation + firstaxis, parity, repetition, frame = axes + + i = firstaxis + 1 + j = _NEXT_AXIS[i+parity-1] + 1 + k = _NEXT_AXIS[i-parity] + 1 + + if frame: + ai, ak = ak, ai + if parity: + aj = -aj + + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + q = numpy.empty((4, )) + if repetition: + q[0] = cj*(cc - ss) + q[i] = cj*(cs + sc) + q[j] = sj*(cc + ss) + q[k] = sj*(cs - sc) + else: + q[0] = cj*cc + sj*ss + q[i] = cj*sc - sj*cs + q[j] = cj*ss + sj*cc + q[k] = cj*cs - sj*sc + if parity: + q[j] *= -1.0 + + return q + + +def quaternion_about_axis(angle, axis): + """Return quaternion for rotation about axis. + + >>> q = quaternion_about_axis(0.123, [1, 0, 0]) + >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0]) + True + + """ + q = numpy.array([0.0, axis[0], axis[1], axis[2]]) + qlen = vector_norm(q) + if qlen > _EPS: + q *= math.sin(angle/2.0) / qlen + q[0] = math.cos(angle/2.0) + return q + + +def quaternion_matrix(quaternion): + """Return homogeneous rotation matrix from quaternion. + + >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0]) + >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0])) + True + >>> M = quaternion_matrix([1, 0, 0, 0]) + >>> numpy.allclose(M, numpy.identity(4)) + True + >>> M = quaternion_matrix([0, 1, 0, 0]) + >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1])) + True + + """ + q = numpy.array(quaternion, dtype=numpy.float64, copy=True) + n = numpy.dot(q, q) + if n < _EPS: + return numpy.identity(4) + q *= math.sqrt(2.0 / n) + q = numpy.outer(q, q) + return numpy.array([ + [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], + [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], + [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], + [ 0.0, 0.0, 0.0, 1.0]]) + + +def quaternion_from_matrix(matrix, isprecise=False): + """Return quaternion from rotation matrix. + + If isprecise is True, the input matrix is assumed to be a precise rotation + matrix and a faster algorithm is used. + + >>> q = quaternion_from_matrix(numpy.identity(4), True) + >>> numpy.allclose(q, [1, 0, 0, 0]) + True + >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1])) + >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0]) + True + >>> R = rotation_matrix(0.123, (1, 2, 3)) + >>> q = quaternion_from_matrix(R, True) + >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786]) + True + >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0], + ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]] + >>> q = quaternion_from_matrix(R) + >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611]) + True + >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0], + ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]] + >>> q = quaternion_from_matrix(R) + >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603]) + True + >>> R = random_rotation_matrix() + >>> q = quaternion_from_matrix(R) + >>> is_same_transform(R, quaternion_matrix(q)) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] + if isprecise: + q = numpy.empty((4, )) + t = numpy.trace(M) + if t > M[3, 3]: + q[0] = t + q[3] = M[1, 0] - M[0, 1] + q[2] = M[0, 2] - M[2, 0] + q[1] = M[2, 1] - M[1, 2] + else: + i, j, k = 1, 2, 3 + if M[1, 1] > M[0, 0]: + i, j, k = 2, 3, 1 + if M[2, 2] > M[i, i]: + i, j, k = 3, 1, 2 + t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] + q[i] = t + q[j] = M[i, j] + M[j, i] + q[k] = M[k, i] + M[i, k] + q[3] = M[k, j] - M[j, k] + q *= 0.5 / math.sqrt(t * M[3, 3]) + else: + m00 = M[0, 0] + m01 = M[0, 1] + m02 = M[0, 2] + m10 = M[1, 0] + m11 = M[1, 1] + m12 = M[1, 2] + m20 = M[2, 0] + m21 = M[2, 1] + m22 = M[2, 2] + # symmetric matrix K + K = numpy.array([[m00-m11-m22, 0.0, 0.0, 0.0], + [m01+m10, m11-m00-m22, 0.0, 0.0], + [m02+m20, m12+m21, m22-m00-m11, 0.0], + [m21-m12, m02-m20, m10-m01, m00+m11+m22]]) + K /= 3.0 + # quaternion is eigenvector of K that corresponds to largest eigenvalue + w, V = numpy.linalg.eigh(K) + q = V[[3, 0, 1, 2], numpy.argmax(w)] + if q[0] < 0.0: + numpy.negative(q, q) + return q + + +def quaternion_multiply(quaternion1, quaternion0): + """Return multiplication of two quaternions. + + >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7]) + >>> numpy.allclose(q, [28, -44, -14, 48]) + True + + """ + w0, x0, y0, z0 = quaternion0 + w1, x1, y1, z1 = quaternion1 + return numpy.array([-x1*x0 - y1*y0 - z1*z0 + w1*w0, + x1*w0 + y1*z0 - z1*y0 + w1*x0, + -x1*z0 + y1*w0 + z1*x0 + w1*y0, + x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=numpy.float64) + + +def quaternion_conjugate(quaternion): + """Return conjugate of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_conjugate(q0) + >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:]) + True + + """ + q = numpy.array(quaternion, dtype=numpy.float64, copy=True) + numpy.negative(q[1:], q[1:]) + return q + + +def quaternion_inverse(quaternion): + """Return inverse of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_inverse(q0) + >>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0]) + True + + """ + q = numpy.array(quaternion, dtype=numpy.float64, copy=True) + numpy.negative(q[1:], q[1:]) + return q / numpy.dot(q, q) + + +def quaternion_real(quaternion): + """Return real part of quaternion. + + >>> quaternion_real([3, 0, 1, 2]) + 3.0 + + """ + return float(quaternion[0]) + + +def quaternion_imag(quaternion): + """Return imaginary part of quaternion. + + >>> quaternion_imag([3, 0, 1, 2]) + array([ 0., 1., 2.]) + + """ + return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True) + + +def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): + """Return spherical linear interpolation between two quaternions. + + >>> q0 = random_quaternion() + >>> q1 = random_quaternion() + >>> q = quaternion_slerp(q0, q1, 0) + >>> numpy.allclose(q, q0) + True + >>> q = quaternion_slerp(q0, q1, 1, 1) + >>> numpy.allclose(q, q1) + True + >>> q = quaternion_slerp(q0, q1, 0.5) + >>> angle = math.acos(numpy.dot(q0, q)) + >>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \ + numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle) + True + + """ + q0 = unit_vector(quat0[:4]) + q1 = unit_vector(quat1[:4]) + if fraction == 0.0: + return q0 + elif fraction == 1.0: + return q1 + d = numpy.dot(q0, q1) + if abs(abs(d) - 1.0) < _EPS: + return q0 + if shortestpath and d < 0.0: + # invert rotation + d = -d + numpy.negative(q1, q1) + angle = math.acos(d) + spin * math.pi + if abs(angle) < _EPS: + return q0 + isin = 1.0 / math.sin(angle) + q0 *= math.sin((1.0 - fraction) * angle) * isin + q1 *= math.sin(fraction * angle) * isin + q0 += q1 + return q0 + + +def random_quaternion(rand=None): + """Return uniform random unit quaternion. + + rand: array like or None + Three independent random variables that are uniformly distributed + between 0 and 1. + + >>> q = random_quaternion() + >>> numpy.allclose(1, vector_norm(q)) + True + >>> q = random_quaternion(numpy.random.random(3)) + >>> len(q.shape), q.shape[0]==4 + (1, True) + + """ + if rand is None: + rand = numpy.random.rand(3) + else: + assert len(rand) == 3 + r1 = numpy.sqrt(1.0 - rand[0]) + r2 = numpy.sqrt(rand[0]) + pi2 = math.pi * 2.0 + t1 = pi2 * rand[1] + t2 = pi2 * rand[2] + return numpy.array([numpy.cos(t2)*r2, numpy.sin(t1)*r1, + numpy.cos(t1)*r1, numpy.sin(t2)*r2]) + + +def random_rotation_matrix(rand=None): + """Return uniform random rotation matrix. + + rand: array like + Three independent random variables that are uniformly distributed + between 0 and 1 for each returned quaternion. + + >>> R = random_rotation_matrix() + >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) + True + + """ + return quaternion_matrix(random_quaternion(rand)) + + +class Arcball(object): + """Virtual Trackball Control. + + >>> ball = Arcball() + >>> ball = Arcball(initial=numpy.identity(4)) + >>> ball.place([320, 320], 320) + >>> ball.down([500, 250]) + >>> ball.drag([475, 275]) + >>> R = ball.matrix() + >>> numpy.allclose(numpy.sum(R), 3.90583455) + True + >>> ball = Arcball(initial=[1, 0, 0, 0]) + >>> ball.place([320, 320], 320) + >>> ball.setaxes([1, 1, 0], [-1, 1, 0]) + >>> ball.setconstrain(True) + >>> ball.down([400, 200]) + >>> ball.drag([200, 400]) + >>> R = ball.matrix() + >>> numpy.allclose(numpy.sum(R), 0.2055924) + True + >>> ball.next() + + """ + def __init__(self, initial=None): + """Initialize virtual trackball control. + + initial : quaternion or rotation matrix + + """ + self._axis = None + self._axes = None + self._radius = 1.0 + self._center = [0.0, 0.0] + self._vdown = numpy.array([0.0, 0.0, 1.0]) + self._constrain = False + if initial is None: + self._qdown = numpy.array([1.0, 0.0, 0.0, 0.0]) + else: + initial = numpy.array(initial, dtype=numpy.float64) + if initial.shape == (4, 4): + self._qdown = quaternion_from_matrix(initial) + elif initial.shape == (4, ): + initial /= vector_norm(initial) + self._qdown = initial + else: + raise ValueError("initial not a quaternion or matrix") + self._qnow = self._qpre = self._qdown + + def place(self, center, radius): + """Place Arcball, e.g. when window size changes. + + center : sequence[2] + Window coordinates of trackball center. + radius : float + Radius of trackball in window coordinates. + + """ + self._radius = float(radius) + self._center[0] = center[0] + self._center[1] = center[1] + + def setaxes(self, *axes): + """Set axes to constrain rotations.""" + if axes is None: + self._axes = None + else: + self._axes = [unit_vector(axis) for axis in axes] + + def setconstrain(self, constrain): + """Set state of constrain to axis mode.""" + self._constrain = bool(constrain) + + def getconstrain(self): + """Return state of constrain to axis mode.""" + return self._constrain + + def down(self, point): + """Set initial cursor window coordinates and pick constrain-axis.""" + self._vdown = arcball_map_to_sphere(point, self._center, self._radius) + self._qdown = self._qpre = self._qnow + if self._constrain and self._axes is not None: + self._axis = arcball_nearest_axis(self._vdown, self._axes) + self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) + else: + self._axis = None + + def drag(self, point): + """Update current cursor window coordinates.""" + vnow = arcball_map_to_sphere(point, self._center, self._radius) + if self._axis is not None: + vnow = arcball_constrain_to_axis(vnow, self._axis) + self._qpre = self._qnow + t = numpy.cross(self._vdown, vnow) + if numpy.dot(t, t) < _EPS: + self._qnow = self._qdown + else: + q = [numpy.dot(self._vdown, vnow), t[0], t[1], t[2]] + self._qnow = quaternion_multiply(q, self._qdown) + + def next(self, acceleration=0.0): + """Continue rotation in direction of last drag.""" + q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) + self._qpre, self._qnow = self._qnow, q + + def matrix(self): + """Return homogeneous rotation matrix.""" + return quaternion_matrix(self._qnow) + + +def arcball_map_to_sphere(point, center, radius): + """Return unit sphere coordinates from window coordinates.""" + v0 = (point[0] - center[0]) / radius + v1 = (center[1] - point[1]) / radius + n = v0*v0 + v1*v1 + if n > 1.0: + # position outside of sphere + n = math.sqrt(n) + return numpy.array([v0/n, v1/n, 0.0]) + else: + return numpy.array([v0, v1, math.sqrt(1.0 - n)]) + + +def arcball_constrain_to_axis(point, axis): + """Return sphere point perpendicular to axis.""" + v = numpy.array(point, dtype=numpy.float64, copy=True) + a = numpy.array(axis, dtype=numpy.float64, copy=True) + v -= a * numpy.dot(a, v) # on plane + n = vector_norm(v) + if n > _EPS: + if v[2] < 0.0: + numpy.negative(v, v) + v /= n + return v + if a[2] == 1.0: + return numpy.array([1.0, 0.0, 0.0]) + return unit_vector([-a[1], a[0], 0.0]) + + +def arcball_nearest_axis(point, axes): + """Return axis, which arc is nearest to point.""" + point = numpy.array(point, dtype=numpy.float64, copy=False) + nearest = None + mx = -1.0 + for axis in axes: + t = numpy.dot(arcball_constrain_to_axis(point, axis), point) + if t > mx: + nearest = axis + mx = t + return nearest + + +# epsilon for testing whether a number is close to zero +_EPS = numpy.finfo(float).eps * 4.0 + +# axis sequences for Euler angles +_NEXT_AXIS = [1, 2, 0, 1] + +# map axes strings to/from tuples of inner axis, parity, repetition, frame +_AXES2TUPLE = { + 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), + 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), + 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), + 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), + 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), + 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), + 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), + 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} + +_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) + + +def vector_norm(data, axis=None, out=None): + """Return length, i.e. Euclidean norm, of ndarray along axis. + + >>> v = numpy.random.random(3) + >>> n = vector_norm(v) + >>> numpy.allclose(n, numpy.linalg.norm(v)) + True + >>> v = numpy.random.rand(6, 5, 3) + >>> n = vector_norm(v, axis=-1) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) + True + >>> n = vector_norm(v, axis=1) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) + True + >>> v = numpy.random.rand(5, 4, 3) + >>> n = numpy.empty((5, 3)) + >>> vector_norm(v, axis=1, out=n) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) + True + >>> vector_norm([]) + 0.0 + >>> vector_norm([1]) + 1.0 + + """ + data = numpy.array(data, dtype=numpy.float64, copy=True) + if out is None: + if data.ndim == 1: + return math.sqrt(numpy.dot(data, data)) + data *= data + out = numpy.atleast_1d(numpy.sum(data, axis=axis)) + numpy.sqrt(out, out) + return out + else: + data *= data + numpy.sum(data, axis=axis, out=out) + numpy.sqrt(out, out) + + +def unit_vector(data, axis=None, out=None): + """Return ndarray normalized by length, i.e. Euclidean norm, along axis. + + >>> v0 = numpy.random.random(3) + >>> v1 = unit_vector(v0) + >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) + True + >>> v0 = numpy.random.rand(5, 4, 3) + >>> v1 = unit_vector(v0, axis=-1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) + >>> numpy.allclose(v1, v2) + True + >>> v1 = unit_vector(v0, axis=1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) + >>> numpy.allclose(v1, v2) + True + >>> v1 = numpy.empty((5, 4, 3)) + >>> unit_vector(v0, axis=1, out=v1) + >>> numpy.allclose(v1, v2) + True + >>> list(unit_vector([])) + [] + >>> list(unit_vector([1])) + [1.0] + + """ + if out is None: + data = numpy.array(data, dtype=numpy.float64, copy=True) + if data.ndim == 1: + data /= math.sqrt(numpy.dot(data, data)) + return data + else: + if out is not data: + out[:] = numpy.array(data, copy=False) + data = out + length = numpy.atleast_1d(numpy.sum(data*data, axis)) + numpy.sqrt(length, length) + if axis is not None: + length = numpy.expand_dims(length, axis) + data /= length + if out is None: + return data + + +def random_vector(size): + """Return array of random doubles in the half-open interval [0.0, 1.0). + + >>> v = random_vector(10000) + >>> numpy.all(v >= 0) and numpy.all(v < 1) + True + >>> v0 = random_vector(10) + >>> v1 = random_vector(10) + >>> numpy.any(v0 == v1) + False + + """ + return numpy.random.random(size) + + +def vector_product(v0, v1, axis=0): + """Return vector perpendicular to vectors. + + >>> v = vector_product([2, 0, 0], [0, 3, 0]) + >>> numpy.allclose(v, [0, 0, 6]) + True + >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] + >>> v1 = [[3], [0], [0]] + >>> v = vector_product(v0, v1) + >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]]) + True + >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] + >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] + >>> v = vector_product(v0, v1, axis=1) + >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]]) + True + + """ + return numpy.cross(v0, v1, axis=axis) + + +def angle_between_vectors(v0, v1, directed=True, axis=0): + """Return angle between vectors. + + If directed is False, the input vectors are interpreted as undirected axes, + i.e. the maximum angle is pi/2. + + >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3]) + >>> numpy.allclose(a, math.pi) + True + >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False) + >>> numpy.allclose(a, 0) + True + >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] + >>> v1 = [[3], [0], [0]] + >>> a = angle_between_vectors(v0, v1) + >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532]) + True + >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] + >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] + >>> a = angle_between_vectors(v0, v1, axis=1) + >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532]) + True + + """ + v0 = numpy.array(v0, dtype=numpy.float64, copy=False) + v1 = numpy.array(v1, dtype=numpy.float64, copy=False) + dot = numpy.sum(v0 * v1, axis=axis) + dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis) + return numpy.arccos(dot if directed else numpy.fabs(dot)) + + +def inverse_matrix(matrix): + """Return inverse of square transformation matrix. + + >>> M0 = random_rotation_matrix() + >>> M1 = inverse_matrix(M0.T) + >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) + True + >>> for size in range(1, 7): + ... M0 = numpy.random.rand(size, size) + ... M1 = inverse_matrix(M0) + ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size) + + """ + return numpy.linalg.inv(matrix) + + +def concatenate_matrices(*matrices): + """Return concatenation of series of transformation matrices. + + >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 + >>> numpy.allclose(M, concatenate_matrices(M)) + True + >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) + True + + """ + M = numpy.identity(4) + for i in matrices: + M = numpy.dot(M, i) + return M + + +def is_same_transform(matrix0, matrix1): + """Return True if two matrices perform same transformation. + + >>> is_same_transform(numpy.identity(4), numpy.identity(4)) + True + >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) + False + + """ + matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) + matrix0 /= matrix0[3, 3] + matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) + matrix1 /= matrix1[3, 3] + return numpy.allclose(matrix0, matrix1) + + +def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'): + """Try import all public attributes from module into global namespace. + + Existing attributes with name clashes are renamed with prefix. + Attributes starting with underscore are ignored by default. + + Return True on successful import. + + """ + import warnings + from importlib import import_module + try: + if not package: + module = import_module(name) + else: + module = import_module('.' + name, package=package) + except ImportError: + if warn: + warnings.warn("failed to import module %s" % name) + else: + for attr in dir(module): + if ignore and attr.startswith(ignore): + continue + if prefix: + if attr in globals(): + globals()[prefix + attr] = globals()[attr] + elif warn: + warnings.warn("no Python implementation of " + attr) + globals()[attr] = getattr(module, attr) + return True + + +#_import_module('_transformations') + +if __name__ == "__main__": + import doctest + import random # used in doctests + numpy.set_printoptions(suppress=True, precision=5) + doctest.testmod() + diff --git a/src/caffe/py_lane_label_reader/perspective_reader.py b/src/caffe/py_lane_label_reader/perspective_reader.py new file mode 100644 index 00000000000..adbc837eca5 --- /dev/null +++ b/src/caffe/py_lane_label_reader/perspective_reader.py @@ -0,0 +1,9 @@ +import numpy as np +import pickle +__all__=['PerspectiveReader'] + +class PerspectiveReader(): + def __init__(self, distortion_file='/scail/group/deeplearning/driving_data/perspective_transforms.pickle'): + self.Ps = pickle.load(open(distortion_file, 'rb')) + def read(self): + return self.Ps diff --git a/src/caffe/py_lane_label_reader/transformations.py b/src/caffe/py_lane_label_reader/transformations.py new file mode 100644 index 00000000000..c31b66050d8 --- /dev/null +++ b/src/caffe/py_lane_label_reader/transformations.py @@ -0,0 +1,1907 @@ +# -*- coding: utf-8 -*- +# transformations.py + +# Copyright (c) 2006-2013, Christoph Gohlke +# Copyright (c) 2006-2013, The Regents of the University of California +# Produced at the Laboratory for Fluorescence Dynamics +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the copyright holders nor the names of any +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Homogeneous Transformation Matrices and Quaternions. + +A library for calculating 4x4 matrices for translating, rotating, reflecting, +scaling, shearing, projecting, orthogonalizing, and superimposing arrays of +3D homogeneous coordinates as well as for converting between rotation matrices, +Euler angles, and quaternions. Also includes an Arcball control object and +functions to decompose transformation matrices. + +:Author: + `Christoph Gohlke `_ + +:Organization: + Laboratory for Fluorescence Dynamics, University of California, Irvine + +:Version: 2013.04.16 + +Requirements +------------ +* `CPython 2.7, 3.2 or 3.3 `_ +* `Numpy 1.7 `_ +* `Transformations.c 2013.01.18 `_ + (recommended for speedup of some functions) + +Notes +----- +The API is not stable yet and is expected to change between revisions. + +This Python code is not optimized for speed. Refer to the transformations.c +module for a faster implementation of some functions. + +Documentation in HTML format can be generated with epydoc. + +Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using +numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using +numpy.dot(M, v) for shape (4, \*) column vectors, respectively +numpy.dot(v, M.T) for shape (\*, 4) row vectors ("array of points"). + +This module follows the "column vectors on the right" and "row major storage" +(C contiguous) conventions. The translation components are in the right column +of the transformation matrix, i.e. M[:3, 3]. +The transpose of the transformation matrices may have to be used to interface +with other graphics systems, e.g. with OpenGL's glMultMatrixd(). See also [16]. + +Calculations are carried out with numpy.float64 precision. + +Vector, point, quaternion, and matrix function arguments are expected to be +"array like", i.e. tuple, list, or numpy arrays. + +Return types are numpy arrays unless specified otherwise. + +Angles are in radians unless specified otherwise. + +Quaternions w+ix+jy+kz are represented as [w, x, y, z]. + +A triple of Euler angles can be applied/interpreted in 24 ways, which can +be specified using a 4 character string or encoded 4-tuple: + + *Axes 4-string*: e.g. 'sxyz' or 'ryxy' + + - first character : rotations are applied to 's'tatic or 'r'otating frame + - remaining characters : successive rotation axis 'x', 'y', or 'z' + + *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) + + - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. + - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed + by 'z', or 'z' is followed by 'x'. Otherwise odd (1). + - repetition : first and last axis are same (1) or different (0). + - frame : rotations are applied to static (0) or rotating (1) frame. + +References +---------- +(1) Matrices and transformations. Ronald Goldman. + In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. +(2) More matrices and transformations: shear and pseudo-perspective. + Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. +(3) Decomposing a matrix into simple transformations. Spencer Thomas. + In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. +(4) Recovering the data from the transformation matrix. Ronald Goldman. + In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. +(5) Euler angle conversion. Ken Shoemake. + In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. +(6) Arcball rotation control. Ken Shoemake. + In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. +(7) Representing attitude: Euler angles, unit quaternions, and rotation + vectors. James Diebel. 2006. +(8) A discussion of the solution for the best rotation to relate two sets + of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. +(9) Closed-form solution of absolute orientation using unit quaternions. + BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642. +(10) Quaternions. Ken Shoemake. + http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf +(11) From quaternion to matrix and back. JMP van Waveren. 2005. + http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm +(12) Uniform random rotations. Ken Shoemake. + In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. +(13) Quaternion in molecular modeling. CFF Karney. + J Mol Graph Mod, 25(5):595-604 +(14) New method for extracting the quaternion from a rotation matrix. + Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087. +(15) Multiple View Geometry in Computer Vision. Hartley and Zissermann. + Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130. +(16) Column Vectors vs. Row Vectors. + http://steve.hollasch.net/cgindex/math/matrix/column-vec.html + +Examples +-------- +>>> alpha, beta, gamma = 0.123, -1.234, 2.345 +>>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] +>>> I = identity_matrix() +>>> Rx = rotation_matrix(alpha, xaxis) +>>> Ry = rotation_matrix(beta, yaxis) +>>> Rz = rotation_matrix(gamma, zaxis) +>>> R = concatenate_matrices(Rx, Ry, Rz) +>>> euler = euler_from_matrix(R, 'rxyz') +>>> numpy.allclose([alpha, beta, gamma], euler) +True +>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') +>>> is_same_transform(R, Re) +True +>>> al, be, ga = euler_from_matrix(Re, 'rxyz') +>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) +True +>>> qx = quaternion_about_axis(alpha, xaxis) +>>> qy = quaternion_about_axis(beta, yaxis) +>>> qz = quaternion_about_axis(gamma, zaxis) +>>> q = quaternion_multiply(qx, qy) +>>> q = quaternion_multiply(q, qz) +>>> Rq = quaternion_matrix(q) +>>> is_same_transform(R, Rq) +True +>>> S = scale_matrix(1.23, origin) +>>> T = translation_matrix([1, 2, 3]) +>>> Z = shear_matrix(beta, xaxis, origin, zaxis) +>>> R = random_rotation_matrix(numpy.random.rand(3)) +>>> M = concatenate_matrices(T, R, Z, S) +>>> scale, shear, angles, trans, persp = decompose_matrix(M) +>>> numpy.allclose(scale, 1.23) +True +>>> numpy.allclose(trans, [1, 2, 3]) +True +>>> numpy.allclose(shear, [0, math.tan(beta), 0]) +True +>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) +True +>>> M1 = compose_matrix(scale, shear, angles, trans, persp) +>>> is_same_transform(M, M1) +True +>>> v0, v1 = random_vector(3), random_vector(3) +>>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1)) +>>> v2 = numpy.dot(v0, M[:3,:3].T) +>>> numpy.allclose(unit_vector(v1), unit_vector(v2)) +True + +""" + +from __future__ import division, print_function + +import math + +import numpy + +__version__ = '2013.04.16' +__docformat__ = 'restructuredtext en' +__all__ = [] + + +def identity_matrix(): + """Return 4x4 identity/unit matrix. + + >>> I = identity_matrix() + >>> numpy.allclose(I, numpy.dot(I, I)) + True + >>> numpy.sum(I), numpy.trace(I) + (4.0, 4.0) + >>> numpy.allclose(I, numpy.identity(4)) + True + + """ + return numpy.identity(4) + + +def translation_matrix(direction): + """Return matrix to translate by direction vector. + + >>> v = numpy.random.random(3) - 0.5 + >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) + True + + """ + M = numpy.identity(4) + M[:3, 3] = direction[:3] + return M + + +def translation_from_matrix(matrix): + """Return translation vector from translation matrix. + + >>> v0 = numpy.random.random(3) - 0.5 + >>> v1 = translation_from_matrix(translation_matrix(v0)) + >>> numpy.allclose(v0, v1) + True + + """ + return numpy.array(matrix, copy=False)[:3, 3].copy() + + +def reflection_matrix(point, normal): + """Return matrix to mirror at plane defined by point and normal vector. + + >>> v0 = numpy.random.random(4) - 0.5 + >>> v0[3] = 1. + >>> v1 = numpy.random.random(3) - 0.5 + >>> R = reflection_matrix(v0, v1) + >>> numpy.allclose(2, numpy.trace(R)) + True + >>> numpy.allclose(v0, numpy.dot(R, v0)) + True + >>> v2 = v0.copy() + >>> v2[:3] += v1 + >>> v3 = v0.copy() + >>> v2[:3] -= v1 + >>> numpy.allclose(v2, numpy.dot(R, v3)) + True + + """ + normal = unit_vector(normal[:3]) + M = numpy.identity(4) + M[:3, :3] -= 2.0 * numpy.outer(normal, normal) + M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal + return M + + +def reflection_from_matrix(matrix): + """Return mirror plane point and normal vector from reflection matrix. + + >>> v0 = numpy.random.random(3) - 0.5 + >>> v1 = numpy.random.random(3) - 0.5 + >>> M0 = reflection_matrix(v0, v1) + >>> point, normal = reflection_from_matrix(M0) + >>> M1 = reflection_matrix(point, normal) + >>> is_same_transform(M0, M1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + # normal: unit eigenvector corresponding to eigenvalue -1 + w, V = numpy.linalg.eig(M[:3, :3]) + i = numpy.where(abs(numpy.real(w) + 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue -1") + normal = numpy.real(V[:, i[0]]).squeeze() + # point: any unit eigenvector corresponding to eigenvalue 1 + w, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + return point, normal + + +def rotation_matrix(angle, direction, point=None): + """Return matrix to rotate about axis defined by point and direction. + + >>> R = rotation_matrix(math.pi/2, [0, 0, 1], [1, 0, 0]) + >>> numpy.allclose(numpy.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1]) + True + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) + >>> is_same_transform(R0, R1) + True + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(-angle, -direc, point) + >>> is_same_transform(R0, R1) + True + >>> I = numpy.identity(4, numpy.float64) + >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) + True + >>> numpy.allclose(2, numpy.trace(rotation_matrix(math.pi/2, + ... direc, point))) + True + + """ + sina = math.sin(angle) + cosa = math.cos(angle) + direction = unit_vector(direction[:3]) + # rotation matrix around unit vector + R = numpy.diag([cosa, cosa, cosa]) + R += numpy.outer(direction, direction) * (1.0 - cosa) + direction *= sina + R += numpy.array([[ 0.0, -direction[2], direction[1]], + [ direction[2], 0.0, -direction[0]], + [-direction[1], direction[0], 0.0]]) + M = numpy.identity(4) + M[:3, :3] = R + if point is not None: + # rotation not around origin + point = numpy.array(point[:3], dtype=numpy.float64, copy=False) + M[:3, 3] = point - numpy.dot(R, point) + return M + + +def rotation_from_matrix(matrix): + """Return rotation angle and axis from rotation matrix. + + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> angle, direc, point = rotation_from_matrix(R0) + >>> R1 = rotation_matrix(angle, direc, point) + >>> is_same_transform(R0, R1) + True + + """ + R = numpy.array(matrix, dtype=numpy.float64, copy=False) + R33 = R[:3, :3] + # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 + w, W = numpy.linalg.eig(R33.T) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + direction = numpy.real(W[:, i[-1]]).squeeze() + # point: unit eigenvector of R33 corresponding to eigenvalue of 1 + w, Q = numpy.linalg.eig(R) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + point = numpy.real(Q[:, i[-1]]).squeeze() + point /= point[3] + # rotation angle depending on direction + cosa = (numpy.trace(R33) - 1.0) / 2.0 + if abs(direction[2]) > 1e-8: + sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] + elif abs(direction[1]) > 1e-8: + sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] + else: + sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] + angle = math.atan2(sina, cosa) + return angle, direction, point + + +def scale_matrix(factor, origin=None, direction=None): + """Return matrix to scale by factor around origin in direction. + + Use factor -1 for point symmetry. + + >>> v = (numpy.random.rand(4, 5) - 0.5) * 20 + >>> v[3] = 1 + >>> S = scale_matrix(-1.234) + >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) + True + >>> factor = random.random() * 10 - 5 + >>> origin = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> S = scale_matrix(factor, origin) + >>> S = scale_matrix(factor, origin, direct) + + """ + if direction is None: + # uniform scaling + M = numpy.diag([factor, factor, factor, 1.0]) + if origin is not None: + M[:3, 3] = origin[:3] + M[:3, 3] *= 1.0 - factor + else: + # nonuniform scaling + direction = unit_vector(direction[:3]) + factor = 1.0 - factor + M = numpy.identity(4) + M[:3, :3] -= factor * numpy.outer(direction, direction) + if origin is not None: + M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction + return M + + +def scale_from_matrix(matrix): + """Return scaling factor, origin and direction from scaling matrix. + + >>> factor = random.random() * 10 - 5 + >>> origin = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> S0 = scale_matrix(factor, origin) + >>> factor, origin, direction = scale_from_matrix(S0) + >>> S1 = scale_matrix(factor, origin, direction) + >>> is_same_transform(S0, S1) + True + >>> S0 = scale_matrix(factor, origin, direct) + >>> factor, origin, direction = scale_from_matrix(S0) + >>> S1 = scale_matrix(factor, origin, direction) + >>> is_same_transform(S0, S1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + factor = numpy.trace(M33) - 2.0 + try: + # direction: unit eigenvector corresponding to eigenvalue factor + w, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(w) - factor) < 1e-8)[0][0] + direction = numpy.real(V[:, i]).squeeze() + direction /= vector_norm(direction) + except IndexError: + # uniform scaling + factor = (factor + 2.0) / 3.0 + direction = None + # origin: any eigenvector corresponding to eigenvalue 1 + w, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 1") + origin = numpy.real(V[:, i[-1]]).squeeze() + origin /= origin[3] + return factor, origin, direction + + +def projection_matrix(point, normal, direction=None, + perspective=None, pseudo=False): + """Return matrix to project onto plane defined by point and normal. + + Using either perspective point, projection direction, or none of both. + + If pseudo is True, perspective projections will preserve relative depth + such that Perspective = dot(Orthogonal, PseudoPerspective). + + >>> P = projection_matrix([0, 0, 0], [1, 0, 0]) + >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) + True + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(3) - 0.5 + >>> P0 = projection_matrix(point, normal) + >>> P1 = projection_matrix(point, normal, direction=direct) + >>> P2 = projection_matrix(point, normal, perspective=persp) + >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) + >>> is_same_transform(P2, numpy.dot(P0, P3)) + True + >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0]) + >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20 + >>> v0[3] = 1 + >>> v1 = numpy.dot(P, v0) + >>> numpy.allclose(v1[1], v0[1]) + True + >>> numpy.allclose(v1[0], 3-v1[1]) + True + + """ + M = numpy.identity(4) + point = numpy.array(point[:3], dtype=numpy.float64, copy=False) + normal = unit_vector(normal[:3]) + if perspective is not None: + # perspective projection + perspective = numpy.array(perspective[:3], dtype=numpy.float64, + copy=False) + M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) + M[:3, :3] -= numpy.outer(perspective, normal) + if pseudo: + # preserve relative depth + M[:3, :3] -= numpy.outer(normal, normal) + M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) + else: + M[:3, 3] = numpy.dot(point, normal) * perspective + M[3, :3] = -normal + M[3, 3] = numpy.dot(perspective, normal) + elif direction is not None: + # parallel projection + direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) + scale = numpy.dot(direction, normal) + M[:3, :3] -= numpy.outer(direction, normal) / scale + M[:3, 3] = direction * (numpy.dot(point, normal) / scale) + else: + # orthogonal projection + M[:3, :3] -= numpy.outer(normal, normal) + M[:3, 3] = numpy.dot(point, normal) * normal + return M + + +def projection_from_matrix(matrix, pseudo=False): + """Return projection plane and perspective point from projection matrix. + + Return values are same as arguments for projection_matrix function: + point, normal, direction, perspective, and pseudo. + + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(3) - 0.5 + >>> P0 = projection_matrix(point, normal) + >>> result = projection_from_matrix(P0) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, direct) + >>> result = projection_from_matrix(P0) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) + >>> result = projection_from_matrix(P0, pseudo=False) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) + >>> result = projection_from_matrix(P0, pseudo=True) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + w, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not pseudo and len(i): + # point: any eigenvector corresponding to eigenvalue 1 + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + # direction: unit eigenvector corresponding to eigenvalue 0 + w, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 0") + direction = numpy.real(V[:, i[0]]).squeeze() + direction /= vector_norm(direction) + # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 + w, V = numpy.linalg.eig(M33.T) + i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] + if len(i): + # parallel projection + normal = numpy.real(V[:, i[0]]).squeeze() + normal /= vector_norm(normal) + return point, normal, direction, None, False + else: + # orthogonal projection, where normal equals direction vector + return point, direction, None, None, False + else: + # perspective projection + i = numpy.where(abs(numpy.real(w)) > 1e-8)[0] + if not len(i): + raise ValueError( + "no eigenvector not corresponding to eigenvalue 0") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + normal = - M[3, :3] + perspective = M[:3, 3] / numpy.dot(point[:3], normal) + if pseudo: + perspective -= normal + return point, normal, None, perspective, pseudo + + +def clip_matrix(left, right, bottom, top, near, far, perspective=False): + """Return matrix to obtain normalized device coordinates from frustum. + + The frustum bounds are axis-aligned along x (left, right), + y (bottom, top) and z (near, far). + + Normalized device coordinates are in range [-1, 1] if coordinates are + inside the frustum. + + If perspective is True the frustum is a truncated pyramid with the + perspective point at origin and direction along z axis, otherwise an + orthographic canonical view volume (a box). + + Homogeneous coordinates transformed by the perspective clip matrix + need to be dehomogenized (divided by w coordinate). + + >>> frustum = numpy.random.rand(6) + >>> frustum[1] += frustum[0] + >>> frustum[3] += frustum[2] + >>> frustum[5] += frustum[4] + >>> M = clip_matrix(perspective=False, *frustum) + >>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) + array([-1., -1., -1., 1.]) + >>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1]) + array([ 1., 1., 1., 1.]) + >>> M = clip_matrix(perspective=True, *frustum) + >>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) + >>> v / v[3] + array([-1., -1., -1., 1.]) + >>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1]) + >>> v / v[3] + array([ 1., 1., -1., 1.]) + + """ + if left >= right or bottom >= top or near >= far: + raise ValueError("invalid frustum") + if perspective: + if near <= _EPS: + raise ValueError("invalid frustum: near <= 0") + t = 2.0 * near + M = [[t/(left-right), 0.0, (right+left)/(right-left), 0.0], + [0.0, t/(bottom-top), (top+bottom)/(top-bottom), 0.0], + [0.0, 0.0, (far+near)/(near-far), t*far/(far-near)], + [0.0, 0.0, -1.0, 0.0]] + else: + M = [[2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)], + [0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)], + [0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)], + [0.0, 0.0, 0.0, 1.0]] + return numpy.array(M) + + +def shear_matrix(angle, direction, point, normal): + """Return matrix to shear by angle along direction vector on shear plane. + + The shear plane is defined by a point and normal vector. The direction + vector must be orthogonal to the plane's normal vector. + + A point P is transformed by the shear matrix into P" such that + the vector P-P" is parallel to the direction vector and its extent is + given by the angle of P-P'-P", where P' is the orthogonal projection + of P onto the shear plane. + + >>> angle = (random.random() - 0.5) * 4*math.pi + >>> direct = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.cross(direct, numpy.random.random(3)) + >>> S = shear_matrix(angle, direct, point, normal) + >>> numpy.allclose(1, numpy.linalg.det(S)) + True + + """ + normal = unit_vector(normal[:3]) + direction = unit_vector(direction[:3]) + if abs(numpy.dot(normal, direction)) > 1e-6: + raise ValueError("direction and normal vectors are not orthogonal") + angle = math.tan(angle) + M = numpy.identity(4) + M[:3, :3] += angle * numpy.outer(direction, normal) + M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction + return M + + +def shear_from_matrix(matrix): + """Return shear angle, direction and plane from shear matrix. + + >>> angle = (random.random() - 0.5) * 4*math.pi + >>> direct = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.cross(direct, numpy.random.random(3)) + >>> S0 = shear_matrix(angle, direct, point, normal) + >>> angle, direct, point, normal = shear_from_matrix(S0) + >>> S1 = shear_matrix(angle, direct, point, normal) + >>> is_same_transform(S0, S1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + # normal: cross independent eigenvectors corresponding to the eigenvalue 1 + w, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-4)[0] + if len(i) < 2: + raise ValueError("no two linear independent eigenvectors found %s" % w) + V = numpy.real(V[:, i]).squeeze().T + lenorm = -1.0 + for i0, i1 in ((0, 1), (0, 2), (1, 2)): + n = numpy.cross(V[i0], V[i1]) + w = vector_norm(n) + if w > lenorm: + lenorm = w + normal = n + normal /= lenorm + # direction and angle + direction = numpy.dot(M33 - numpy.identity(3), normal) + angle = vector_norm(direction) + direction /= angle + angle = math.atan(angle) + # point: eigenvector corresponding to eigenvalue 1 + w, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 1") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + return angle, direction, point, normal + + +def decompose_matrix(matrix): + """Return sequence of transformations from transformation matrix. + + matrix : array_like + Non-degenerative homogeneous transformation matrix + + Return tuple of: + scale : vector of 3 scaling factors + shear : list of shear factors for x-y, x-z, y-z axes + angles : list of Euler angles about static x, y, z axes + translate : translation vector along x, y, z axes + perspective : perspective partition of matrix + + Raise ValueError if matrix is of wrong type or degenerative. + + >>> T0 = translation_matrix([1, 2, 3]) + >>> scale, shear, angles, trans, persp = decompose_matrix(T0) + >>> T1 = translation_matrix(trans) + >>> numpy.allclose(T0, T1) + True + >>> S = scale_matrix(0.123) + >>> scale, shear, angles, trans, persp = decompose_matrix(S) + >>> scale[0] + 0.123 + >>> R0 = euler_matrix(1, 2, 3) + >>> scale, shear, angles, trans, persp = decompose_matrix(R0) + >>> R1 = euler_matrix(*angles) + >>> numpy.allclose(R0, R1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=True).T + if abs(M[3, 3]) < _EPS: + raise ValueError("M[3, 3] is zero") + M /= M[3, 3] + P = M.copy() + P[:, 3] = 0.0, 0.0, 0.0, 1.0 + if not numpy.linalg.det(P): + raise ValueError("matrix is singular") + + scale = numpy.zeros((3, )) + shear = [0.0, 0.0, 0.0] + angles = [0.0, 0.0, 0.0] + + if any(abs(M[:3, 3]) > _EPS): + perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) + M[:, 3] = 0.0, 0.0, 0.0, 1.0 + else: + perspective = numpy.array([0.0, 0.0, 0.0, 1.0]) + + translate = M[3, :3].copy() + M[3, :3] = 0.0 + + row = M[:3, :3].copy() + scale[0] = vector_norm(row[0]) + row[0] /= scale[0] + shear[0] = numpy.dot(row[0], row[1]) + row[1] -= row[0] * shear[0] + scale[1] = vector_norm(row[1]) + row[1] /= scale[1] + shear[0] /= scale[1] + shear[1] = numpy.dot(row[0], row[2]) + row[2] -= row[0] * shear[1] + shear[2] = numpy.dot(row[1], row[2]) + row[2] -= row[1] * shear[2] + scale[2] = vector_norm(row[2]) + row[2] /= scale[2] + shear[1:] /= scale[2] + + if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: + numpy.negative(scale, scale) + numpy.negative(row, row) + + angles[1] = math.asin(-row[0, 2]) + if math.cos(angles[1]): + angles[0] = math.atan2(row[1, 2], row[2, 2]) + angles[2] = math.atan2(row[0, 1], row[0, 0]) + else: + #angles[0] = math.atan2(row[1, 0], row[1, 1]) + angles[0] = math.atan2(-row[2, 1], row[1, 1]) + angles[2] = 0.0 + + return scale, shear, angles, translate, perspective + + +def compose_matrix(scale=None, shear=None, angles=None, translate=None, + perspective=None): + """Return transformation matrix from sequence of transformations. + + This is the inverse of the decompose_matrix function. + + Sequence of transformations: + scale : vector of 3 scaling factors + shear : list of shear factors for x-y, x-z, y-z axes + angles : list of Euler angles about static x, y, z axes + translate : translation vector along x, y, z axes + perspective : perspective partition of matrix + + >>> scale = numpy.random.random(3) - 0.5 + >>> shear = numpy.random.random(3) - 0.5 + >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) + >>> trans = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(4) - 0.5 + >>> M0 = compose_matrix(scale, shear, angles, trans, persp) + >>> result = decompose_matrix(M0) + >>> M1 = compose_matrix(*result) + >>> is_same_transform(M0, M1) + True + + """ + M = numpy.identity(4) + if perspective is not None: + P = numpy.identity(4) + P[3, :] = perspective[:4] + M = numpy.dot(M, P) + if translate is not None: + T = numpy.identity(4) + T[:3, 3] = translate[:3] + M = numpy.dot(M, T) + if angles is not None: + R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') + M = numpy.dot(M, R) + if shear is not None: + Z = numpy.identity(4) + Z[1, 2] = shear[2] + Z[0, 2] = shear[1] + Z[0, 1] = shear[0] + M = numpy.dot(M, Z) + if scale is not None: + S = numpy.identity(4) + S[0, 0] = scale[0] + S[1, 1] = scale[1] + S[2, 2] = scale[2] + M = numpy.dot(M, S) + M /= M[3, 3] + return M + + +def orthogonalization_matrix(lengths, angles): + """Return orthogonalization matrix for crystallographic cell coordinates. + + Angles are expected in degrees. + + The de-orthogonalization matrix is the inverse. + + >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90]) + >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) + True + >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) + >>> numpy.allclose(numpy.sum(O), 43.063229) + True + + """ + a, b, c = lengths + angles = numpy.radians(angles) + sina, sinb, _ = numpy.sin(angles) + cosa, cosb, cosg = numpy.cos(angles) + co = (cosa * cosb - cosg) / (sina * sinb) + return numpy.array([ + [ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0], + [-a*sinb*co, b*sina, 0.0, 0.0], + [ a*cosb, b*cosa, c, 0.0], + [ 0.0, 0.0, 0.0, 1.0]]) + + +def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): + """Return affine transform matrix to register two point sets. + + v0 and v1 are shape (ndims, \*) arrays of at least ndims non-homogeneous + coordinates, where ndims is the dimensionality of the coordinate space. + + If shear is False, a similarity transformation matrix is returned. + If also scale is False, a rigid/Euclidean transformation matrix + is returned. + + By default the algorithm by Hartley and Zissermann [15] is used. + If usesvd is True, similarity and Euclidean transformation matrices + are calculated by minimizing the weighted sum of squared deviations + (RMSD) according to the algorithm by Kabsch [8]. + Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9] + is used, which is slower when using this Python implementation. + + The returned matrix performs rotation, translation and uniform scaling + (if specified). + + >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]] + >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]] + >>> affine_matrix_from_points(v0, v1) + array([[ 0.14549, 0.00062, 675.50008], + [ 0.00048, 0.14094, 53.24971], + [ 0. , 0. , 1. ]]) + >>> T = translation_matrix(numpy.random.random(3)-0.5) + >>> R = random_rotation_matrix(numpy.random.random(3)) + >>> S = scale_matrix(random.random()) + >>> M = concatenate_matrices(T, R, S) + >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 + >>> v0[3] = 1 + >>> v1 = numpy.dot(M, v0) + >>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1) + >>> M = affine_matrix_from_points(v0[:3], v1[:3]) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + + More examples in superimposition_matrix() + + """ + v0 = numpy.array(v0, dtype=numpy.float64, copy=True) + v1 = numpy.array(v1, dtype=numpy.float64, copy=True) + + ndims = v0.shape[0] + if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape: + raise ValueError("input arrays are of wrong shape or type") + + # move centroids to origin + t0 = -numpy.mean(v0, axis=1) + M0 = numpy.identity(ndims+1) + M0[:ndims, ndims] = t0 + v0 += t0.reshape(ndims, 1) + t1 = -numpy.mean(v1, axis=1) + M1 = numpy.identity(ndims+1) + M1[:ndims, ndims] = t1 + v1 += t1.reshape(ndims, 1) + + if shear: + # Affine transformation + A = numpy.concatenate((v0, v1), axis=0) + u, s, vh = numpy.linalg.svd(A.T) + vh = vh[:ndims].T + B = vh[:ndims] + C = vh[ndims:2*ndims] + t = numpy.dot(C, numpy.linalg.pinv(B)) + t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1) + M = numpy.vstack((t, ((0.0,)*ndims) + (1.0,))) + elif usesvd or ndims != 3: + # Rigid transformation via SVD of covariance matrix + u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) + # rotation matrix from SVD orthonormal bases + R = numpy.dot(u, vh) + if numpy.linalg.det(R) < 0.0: + # R does not constitute right handed system + R -= numpy.outer(u[:, ndims-1], vh[ndims-1, :]*2.0) + s[-1] *= -1.0 + # homogeneous transformation matrix + M = numpy.identity(ndims+1) + M[:ndims, :ndims] = R + else: + # Rigid transformation matrix via quaternion + # compute symmetric matrix N + xx, yy, zz = numpy.sum(v0 * v1, axis=1) + xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) + xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) + N = [[xx+yy+zz, 0.0, 0.0, 0.0], + [yz-zy, xx-yy-zz, 0.0, 0.0], + [zx-xz, xy+yx, yy-xx-zz, 0.0], + [xy-yx, zx+xz, yz+zy, zz-xx-yy]] + # quaternion: eigenvector corresponding to most positive eigenvalue + w, V = numpy.linalg.eigh(N) + q = V[:, numpy.argmax(w)] + q /= vector_norm(q) # unit quaternion + # homogeneous transformation matrix + M = quaternion_matrix(q) + + if scale and not shear: + # Affine transformation; scale is ratio of RMS deviations from centroid + v0 *= v0 + v1 *= v1 + M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) + + # move centroids back + M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0)) + M /= M[ndims, ndims] + return M + + +def superimposition_matrix(v0, v1, scale=False, usesvd=True): + """Return matrix to transform given 3D point set into second point set. + + v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 points. + + The parameters scale and usesvd are explained in the more general + affine_matrix_from_points function. + + The returned matrix is a similarity or Euclidean transformation matrix. + This function has a fast C implementation in transformations.c. + + >>> v0 = numpy.random.rand(3, 10) + >>> M = superimposition_matrix(v0, v0) + >>> numpy.allclose(M, numpy.identity(4)) + True + >>> R = random_rotation_matrix(numpy.random.random(3)) + >>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]] + >>> v1 = numpy.dot(R, v0) + >>> M = superimposition_matrix(v0, v1) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 + >>> v0[3] = 1 + >>> v1 = numpy.dot(R, v0) + >>> M = superimposition_matrix(v0, v1) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> S = scale_matrix(random.random()) + >>> T = translation_matrix(numpy.random.random(3)-0.5) + >>> M = concatenate_matrices(T, R, S) + >>> v1 = numpy.dot(M, v0) + >>> v0[:3] += numpy.random.normal(0, 1e-9, 300).reshape(3, -1) + >>> M = superimposition_matrix(v0, v1, scale=True) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> v = numpy.empty((4, 100, 3)) + >>> v[:, :, 0] = v0 + >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) + >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) + True + + """ + v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] + v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] + return affine_matrix_from_points(v0, v1, shear=False, + scale=scale, usesvd=usesvd) + + +def euler_matrix(ai, aj, ak, axes='sxyz'): + """Return homogeneous rotation matrix from Euler angles and axis sequence. + + ai, aj, ak : Euler's roll, pitch and yaw angles + axes : One of 24 axis sequences as string or encoded tuple + + >>> R = euler_matrix(1, 2, 3, 'syxz') + >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) + True + >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) + >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) + True + >>> ai, aj, ak = (4*math.pi) * (numpy.random.random(3) - 0.5) + >>> for axes in _AXES2TUPLE.keys(): + ... R = euler_matrix(ai, aj, ak, axes) + >>> for axes in _TUPLE2AXES.keys(): + ... R = euler_matrix(ai, aj, ak, axes) + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] + except (AttributeError, KeyError): + _TUPLE2AXES[axes] # validation + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + if frame: + ai, ak = ak, ai + if parity: + ai, aj, ak = -ai, -aj, -ak + + si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) + ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) + cc, cs = ci*ck, ci*sk + sc, ss = si*ck, si*sk + + M = numpy.identity(4) + if repetition: + M[i, i] = cj + M[i, j] = sj*si + M[i, k] = sj*ci + M[j, i] = sj*sk + M[j, j] = -cj*ss+cc + M[j, k] = -cj*cs-sc + M[k, i] = -sj*ck + M[k, j] = cj*sc+cs + M[k, k] = cj*cc-ss + else: + M[i, i] = cj*ck + M[i, j] = sj*sc-cs + M[i, k] = sj*cc+ss + M[j, i] = cj*sk + M[j, j] = sj*ss+cc + M[j, k] = sj*cs-sc + M[k, i] = -sj + M[k, j] = cj*si + M[k, k] = cj*ci + return M + + +def euler_from_matrix(matrix, axes='sxyz'): + """Return Euler angles from rotation matrix for specified axis sequence. + + axes : One of 24 axis sequences as string or encoded tuple + + Note that many Euler angle triplets can describe one matrix. + + >>> R0 = euler_matrix(1, 2, 3, 'syxz') + >>> al, be, ga = euler_from_matrix(R0, 'syxz') + >>> R1 = euler_matrix(al, be, ga, 'syxz') + >>> numpy.allclose(R0, R1) + True + >>> angles = (4*math.pi) * (numpy.random.random(3) - 0.5) + >>> for axes in _AXES2TUPLE.keys(): + ... R0 = euler_matrix(axes=axes, *angles) + ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) + ... if not numpy.allclose(R0, R1): print(axes, "failed") + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] + except (AttributeError, KeyError): + _TUPLE2AXES[axes] # validation + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] + if repetition: + sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) + if sy > _EPS: + ax = math.atan2( M[i, j], M[i, k]) + ay = math.atan2( sy, M[i, i]) + az = math.atan2( M[j, i], -M[k, i]) + else: + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2( sy, M[i, i]) + az = 0.0 + else: + cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) + if cy > _EPS: + ax = math.atan2( M[k, j], M[k, k]) + ay = math.atan2(-M[k, i], cy) + az = math.atan2( M[j, i], M[i, i]) + else: + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2(-M[k, i], cy) + az = 0.0 + + if parity: + ax, ay, az = -ax, -ay, -az + if frame: + ax, az = az, ax + return ax, ay, az + + +def euler_from_quaternion(quaternion, axes='sxyz'): + """Return Euler angles from quaternion for specified axis sequence. + + >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0]) + >>> numpy.allclose(angles, [0.123, 0, 0]) + True + + """ + return euler_from_matrix(quaternion_matrix(quaternion), axes) + + +def quaternion_from_euler(ai, aj, ak, axes='sxyz'): + """Return quaternion from Euler angles and axis sequence. + + ai, aj, ak : Euler's roll, pitch and yaw angles + axes : One of 24 axis sequences as string or encoded tuple + + >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') + >>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435]) + True + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] + except (AttributeError, KeyError): + _TUPLE2AXES[axes] # validation + firstaxis, parity, repetition, frame = axes + + i = firstaxis + 1 + j = _NEXT_AXIS[i+parity-1] + 1 + k = _NEXT_AXIS[i-parity] + 1 + + if frame: + ai, ak = ak, ai + if parity: + aj = -aj + + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + q = numpy.empty((4, )) + if repetition: + q[0] = cj*(cc - ss) + q[i] = cj*(cs + sc) + q[j] = sj*(cc + ss) + q[k] = sj*(cs - sc) + else: + q[0] = cj*cc + sj*ss + q[i] = cj*sc - sj*cs + q[j] = cj*ss + sj*cc + q[k] = cj*cs - sj*sc + if parity: + q[j] *= -1.0 + + return q + + +def quaternion_about_axis(angle, axis): + """Return quaternion for rotation about axis. + + >>> q = quaternion_about_axis(0.123, [1, 0, 0]) + >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0]) + True + + """ + q = numpy.array([0.0, axis[0], axis[1], axis[2]]) + qlen = vector_norm(q) + if qlen > _EPS: + q *= math.sin(angle/2.0) / qlen + q[0] = math.cos(angle/2.0) + return q + + +def quaternion_matrix(quaternion): + """Return homogeneous rotation matrix from quaternion. + + >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0]) + >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0])) + True + >>> M = quaternion_matrix([1, 0, 0, 0]) + >>> numpy.allclose(M, numpy.identity(4)) + True + >>> M = quaternion_matrix([0, 1, 0, 0]) + >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1])) + True + + """ + q = numpy.array(quaternion, dtype=numpy.float64, copy=True) + n = numpy.dot(q, q) + if n < _EPS: + return numpy.identity(4) + q *= math.sqrt(2.0 / n) + q = numpy.outer(q, q) + return numpy.array([ + [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], + [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], + [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], + [ 0.0, 0.0, 0.0, 1.0]]) + + +def quaternion_from_matrix(matrix, isprecise=False): + """Return quaternion from rotation matrix. + + If isprecise is True, the input matrix is assumed to be a precise rotation + matrix and a faster algorithm is used. + + >>> q = quaternion_from_matrix(numpy.identity(4), True) + >>> numpy.allclose(q, [1, 0, 0, 0]) + True + >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1])) + >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0]) + True + >>> R = rotation_matrix(0.123, (1, 2, 3)) + >>> q = quaternion_from_matrix(R, True) + >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786]) + True + >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0], + ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]] + >>> q = quaternion_from_matrix(R) + >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611]) + True + >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0], + ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]] + >>> q = quaternion_from_matrix(R) + >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603]) + True + >>> R = random_rotation_matrix() + >>> q = quaternion_from_matrix(R) + >>> is_same_transform(R, quaternion_matrix(q)) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] + if isprecise: + q = numpy.empty((4, )) + t = numpy.trace(M) + if t > M[3, 3]: + q[0] = t + q[3] = M[1, 0] - M[0, 1] + q[2] = M[0, 2] - M[2, 0] + q[1] = M[2, 1] - M[1, 2] + else: + i, j, k = 1, 2, 3 + if M[1, 1] > M[0, 0]: + i, j, k = 2, 3, 1 + if M[2, 2] > M[i, i]: + i, j, k = 3, 1, 2 + t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] + q[i] = t + q[j] = M[i, j] + M[j, i] + q[k] = M[k, i] + M[i, k] + q[3] = M[k, j] - M[j, k] + q *= 0.5 / math.sqrt(t * M[3, 3]) + else: + m00 = M[0, 0] + m01 = M[0, 1] + m02 = M[0, 2] + m10 = M[1, 0] + m11 = M[1, 1] + m12 = M[1, 2] + m20 = M[2, 0] + m21 = M[2, 1] + m22 = M[2, 2] + # symmetric matrix K + K = numpy.array([[m00-m11-m22, 0.0, 0.0, 0.0], + [m01+m10, m11-m00-m22, 0.0, 0.0], + [m02+m20, m12+m21, m22-m00-m11, 0.0], + [m21-m12, m02-m20, m10-m01, m00+m11+m22]]) + K /= 3.0 + # quaternion is eigenvector of K that corresponds to largest eigenvalue + w, V = numpy.linalg.eigh(K) + q = V[[3, 0, 1, 2], numpy.argmax(w)] + if q[0] < 0.0: + numpy.negative(q, q) + return q + + +def quaternion_multiply(quaternion1, quaternion0): + """Return multiplication of two quaternions. + + >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7]) + >>> numpy.allclose(q, [28, -44, -14, 48]) + True + + """ + w0, x0, y0, z0 = quaternion0 + w1, x1, y1, z1 = quaternion1 + return numpy.array([-x1*x0 - y1*y0 - z1*z0 + w1*w0, + x1*w0 + y1*z0 - z1*y0 + w1*x0, + -x1*z0 + y1*w0 + z1*x0 + w1*y0, + x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=numpy.float64) + + +def quaternion_conjugate(quaternion): + """Return conjugate of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_conjugate(q0) + >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:]) + True + + """ + q = numpy.array(quaternion, dtype=numpy.float64, copy=True) + numpy.negative(q[1:], q[1:]) + return q + + +def quaternion_inverse(quaternion): + """Return inverse of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_inverse(q0) + >>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0]) + True + + """ + q = numpy.array(quaternion, dtype=numpy.float64, copy=True) + numpy.negative(q[1:], q[1:]) + return q / numpy.dot(q, q) + + +def quaternion_real(quaternion): + """Return real part of quaternion. + + >>> quaternion_real([3, 0, 1, 2]) + 3.0 + + """ + return float(quaternion[0]) + + +def quaternion_imag(quaternion): + """Return imaginary part of quaternion. + + >>> quaternion_imag([3, 0, 1, 2]) + array([ 0., 1., 2.]) + + """ + return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True) + + +def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): + """Return spherical linear interpolation between two quaternions. + + >>> q0 = random_quaternion() + >>> q1 = random_quaternion() + >>> q = quaternion_slerp(q0, q1, 0) + >>> numpy.allclose(q, q0) + True + >>> q = quaternion_slerp(q0, q1, 1, 1) + >>> numpy.allclose(q, q1) + True + >>> q = quaternion_slerp(q0, q1, 0.5) + >>> angle = math.acos(numpy.dot(q0, q)) + >>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \ + numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle) + True + + """ + q0 = unit_vector(quat0[:4]) + q1 = unit_vector(quat1[:4]) + if fraction == 0.0: + return q0 + elif fraction == 1.0: + return q1 + d = numpy.dot(q0, q1) + if abs(abs(d) - 1.0) < _EPS: + return q0 + if shortestpath and d < 0.0: + # invert rotation + d = -d + numpy.negative(q1, q1) + angle = math.acos(d) + spin * math.pi + if abs(angle) < _EPS: + return q0 + isin = 1.0 / math.sin(angle) + q0 *= math.sin((1.0 - fraction) * angle) * isin + q1 *= math.sin(fraction * angle) * isin + q0 += q1 + return q0 + + +def random_quaternion(rand=None): + """Return uniform random unit quaternion. + + rand: array like or None + Three independent random variables that are uniformly distributed + between 0 and 1. + + >>> q = random_quaternion() + >>> numpy.allclose(1, vector_norm(q)) + True + >>> q = random_quaternion(numpy.random.random(3)) + >>> len(q.shape), q.shape[0]==4 + (1, True) + + """ + if rand is None: + rand = numpy.random.rand(3) + else: + assert len(rand) == 3 + r1 = numpy.sqrt(1.0 - rand[0]) + r2 = numpy.sqrt(rand[0]) + pi2 = math.pi * 2.0 + t1 = pi2 * rand[1] + t2 = pi2 * rand[2] + return numpy.array([numpy.cos(t2)*r2, numpy.sin(t1)*r1, + numpy.cos(t1)*r1, numpy.sin(t2)*r2]) + + +def random_rotation_matrix(rand=None): + """Return uniform random rotation matrix. + + rand: array like + Three independent random variables that are uniformly distributed + between 0 and 1 for each returned quaternion. + + >>> R = random_rotation_matrix() + >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) + True + + """ + return quaternion_matrix(random_quaternion(rand)) + + +class Arcball(object): + """Virtual Trackball Control. + + >>> ball = Arcball() + >>> ball = Arcball(initial=numpy.identity(4)) + >>> ball.place([320, 320], 320) + >>> ball.down([500, 250]) + >>> ball.drag([475, 275]) + >>> R = ball.matrix() + >>> numpy.allclose(numpy.sum(R), 3.90583455) + True + >>> ball = Arcball(initial=[1, 0, 0, 0]) + >>> ball.place([320, 320], 320) + >>> ball.setaxes([1, 1, 0], [-1, 1, 0]) + >>> ball.setconstrain(True) + >>> ball.down([400, 200]) + >>> ball.drag([200, 400]) + >>> R = ball.matrix() + >>> numpy.allclose(numpy.sum(R), 0.2055924) + True + >>> ball.next() + + """ + def __init__(self, initial=None): + """Initialize virtual trackball control. + + initial : quaternion or rotation matrix + + """ + self._axis = None + self._axes = None + self._radius = 1.0 + self._center = [0.0, 0.0] + self._vdown = numpy.array([0.0, 0.0, 1.0]) + self._constrain = False + if initial is None: + self._qdown = numpy.array([1.0, 0.0, 0.0, 0.0]) + else: + initial = numpy.array(initial, dtype=numpy.float64) + if initial.shape == (4, 4): + self._qdown = quaternion_from_matrix(initial) + elif initial.shape == (4, ): + initial /= vector_norm(initial) + self._qdown = initial + else: + raise ValueError("initial not a quaternion or matrix") + self._qnow = self._qpre = self._qdown + + def place(self, center, radius): + """Place Arcball, e.g. when window size changes. + + center : sequence[2] + Window coordinates of trackball center. + radius : float + Radius of trackball in window coordinates. + + """ + self._radius = float(radius) + self._center[0] = center[0] + self._center[1] = center[1] + + def setaxes(self, *axes): + """Set axes to constrain rotations.""" + if axes is None: + self._axes = None + else: + self._axes = [unit_vector(axis) for axis in axes] + + def setconstrain(self, constrain): + """Set state of constrain to axis mode.""" + self._constrain = bool(constrain) + + def getconstrain(self): + """Return state of constrain to axis mode.""" + return self._constrain + + def down(self, point): + """Set initial cursor window coordinates and pick constrain-axis.""" + self._vdown = arcball_map_to_sphere(point, self._center, self._radius) + self._qdown = self._qpre = self._qnow + if self._constrain and self._axes is not None: + self._axis = arcball_nearest_axis(self._vdown, self._axes) + self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) + else: + self._axis = None + + def drag(self, point): + """Update current cursor window coordinates.""" + vnow = arcball_map_to_sphere(point, self._center, self._radius) + if self._axis is not None: + vnow = arcball_constrain_to_axis(vnow, self._axis) + self._qpre = self._qnow + t = numpy.cross(self._vdown, vnow) + if numpy.dot(t, t) < _EPS: + self._qnow = self._qdown + else: + q = [numpy.dot(self._vdown, vnow), t[0], t[1], t[2]] + self._qnow = quaternion_multiply(q, self._qdown) + + def next(self, acceleration=0.0): + """Continue rotation in direction of last drag.""" + q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) + self._qpre, self._qnow = self._qnow, q + + def matrix(self): + """Return homogeneous rotation matrix.""" + return quaternion_matrix(self._qnow) + + +def arcball_map_to_sphere(point, center, radius): + """Return unit sphere coordinates from window coordinates.""" + v0 = (point[0] - center[0]) / radius + v1 = (center[1] - point[1]) / radius + n = v0*v0 + v1*v1 + if n > 1.0: + # position outside of sphere + n = math.sqrt(n) + return numpy.array([v0/n, v1/n, 0.0]) + else: + return numpy.array([v0, v1, math.sqrt(1.0 - n)]) + + +def arcball_constrain_to_axis(point, axis): + """Return sphere point perpendicular to axis.""" + v = numpy.array(point, dtype=numpy.float64, copy=True) + a = numpy.array(axis, dtype=numpy.float64, copy=True) + v -= a * numpy.dot(a, v) # on plane + n = vector_norm(v) + if n > _EPS: + if v[2] < 0.0: + numpy.negative(v, v) + v /= n + return v + if a[2] == 1.0: + return numpy.array([1.0, 0.0, 0.0]) + return unit_vector([-a[1], a[0], 0.0]) + + +def arcball_nearest_axis(point, axes): + """Return axis, which arc is nearest to point.""" + point = numpy.array(point, dtype=numpy.float64, copy=False) + nearest = None + mx = -1.0 + for axis in axes: + t = numpy.dot(arcball_constrain_to_axis(point, axis), point) + if t > mx: + nearest = axis + mx = t + return nearest + + +# epsilon for testing whether a number is close to zero +_EPS = numpy.finfo(float).eps * 4.0 + +# axis sequences for Euler angles +_NEXT_AXIS = [1, 2, 0, 1] + +# map axes strings to/from tuples of inner axis, parity, repetition, frame +_AXES2TUPLE = { + 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), + 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), + 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), + 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), + 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), + 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), + 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), + 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} + +_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) + + +def vector_norm(data, axis=None, out=None): + """Return length, i.e. Euclidean norm, of ndarray along axis. + + >>> v = numpy.random.random(3) + >>> n = vector_norm(v) + >>> numpy.allclose(n, numpy.linalg.norm(v)) + True + >>> v = numpy.random.rand(6, 5, 3) + >>> n = vector_norm(v, axis=-1) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) + True + >>> n = vector_norm(v, axis=1) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) + True + >>> v = numpy.random.rand(5, 4, 3) + >>> n = numpy.empty((5, 3)) + >>> vector_norm(v, axis=1, out=n) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) + True + >>> vector_norm([]) + 0.0 + >>> vector_norm([1]) + 1.0 + + """ + data = numpy.array(data, dtype=numpy.float64, copy=True) + if out is None: + if data.ndim == 1: + return math.sqrt(numpy.dot(data, data)) + data *= data + out = numpy.atleast_1d(numpy.sum(data, axis=axis)) + numpy.sqrt(out, out) + return out + else: + data *= data + numpy.sum(data, axis=axis, out=out) + numpy.sqrt(out, out) + + +def unit_vector(data, axis=None, out=None): + """Return ndarray normalized by length, i.e. Euclidean norm, along axis. + + >>> v0 = numpy.random.random(3) + >>> v1 = unit_vector(v0) + >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) + True + >>> v0 = numpy.random.rand(5, 4, 3) + >>> v1 = unit_vector(v0, axis=-1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) + >>> numpy.allclose(v1, v2) + True + >>> v1 = unit_vector(v0, axis=1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) + >>> numpy.allclose(v1, v2) + True + >>> v1 = numpy.empty((5, 4, 3)) + >>> unit_vector(v0, axis=1, out=v1) + >>> numpy.allclose(v1, v2) + True + >>> list(unit_vector([])) + [] + >>> list(unit_vector([1])) + [1.0] + + """ + if out is None: + data = numpy.array(data, dtype=numpy.float64, copy=True) + if data.ndim == 1: + data /= math.sqrt(numpy.dot(data, data)) + return data + else: + if out is not data: + out[:] = numpy.array(data, copy=False) + data = out + length = numpy.atleast_1d(numpy.sum(data*data, axis)) + numpy.sqrt(length, length) + if axis is not None: + length = numpy.expand_dims(length, axis) + data /= length + if out is None: + return data + + +def random_vector(size): + """Return array of random doubles in the half-open interval [0.0, 1.0). + + >>> v = random_vector(10000) + >>> numpy.all(v >= 0) and numpy.all(v < 1) + True + >>> v0 = random_vector(10) + >>> v1 = random_vector(10) + >>> numpy.any(v0 == v1) + False + + """ + return numpy.random.random(size) + + +def vector_product(v0, v1, axis=0): + """Return vector perpendicular to vectors. + + >>> v = vector_product([2, 0, 0], [0, 3, 0]) + >>> numpy.allclose(v, [0, 0, 6]) + True + >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] + >>> v1 = [[3], [0], [0]] + >>> v = vector_product(v0, v1) + >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]]) + True + >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] + >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] + >>> v = vector_product(v0, v1, axis=1) + >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]]) + True + + """ + return numpy.cross(v0, v1, axis=axis) + + +def angle_between_vectors(v0, v1, directed=True, axis=0): + """Return angle between vectors. + + If directed is False, the input vectors are interpreted as undirected axes, + i.e. the maximum angle is pi/2. + + >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3]) + >>> numpy.allclose(a, math.pi) + True + >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False) + >>> numpy.allclose(a, 0) + True + >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] + >>> v1 = [[3], [0], [0]] + >>> a = angle_between_vectors(v0, v1) + >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532]) + True + >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] + >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] + >>> a = angle_between_vectors(v0, v1, axis=1) + >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532]) + True + + """ + v0 = numpy.array(v0, dtype=numpy.float64, copy=False) + v1 = numpy.array(v1, dtype=numpy.float64, copy=False) + dot = numpy.sum(v0 * v1, axis=axis) + dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis) + return numpy.arccos(dot if directed else numpy.fabs(dot)) + + +def inverse_matrix(matrix): + """Return inverse of square transformation matrix. + + >>> M0 = random_rotation_matrix() + >>> M1 = inverse_matrix(M0.T) + >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) + True + >>> for size in range(1, 7): + ... M0 = numpy.random.rand(size, size) + ... M1 = inverse_matrix(M0) + ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size) + + """ + return numpy.linalg.inv(matrix) + + +def concatenate_matrices(*matrices): + """Return concatenation of series of transformation matrices. + + >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 + >>> numpy.allclose(M, concatenate_matrices(M)) + True + >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) + True + + """ + M = numpy.identity(4) + for i in matrices: + M = numpy.dot(M, i) + return M + + +def is_same_transform(matrix0, matrix1): + """Return True if two matrices perform same transformation. + + >>> is_same_transform(numpy.identity(4), numpy.identity(4)) + True + >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) + False + + """ + matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) + matrix0 /= matrix0[3, 3] + matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) + matrix1 /= matrix1[3, 3] + return numpy.allclose(matrix0, matrix1) + + +def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'): + """Try import all public attributes from module into global namespace. + + Existing attributes with name clashes are renamed with prefix. + Attributes starting with underscore are ignored by default. + + Return True on successful import. + + """ + import warnings + from importlib import import_module + try: + if not package: + module = import_module(name) + else: + module = import_module('.' + name, package=package) + except ImportError: + if warn: + warnings.warn("failed to import module %s" % name) + else: + for attr in dir(module): + if ignore and attr.startswith(ignore): + continue + if prefix: + if attr in globals(): + globals()[prefix + attr] = globals()[attr] + elif warn: + warnings.warn("no Python implementation of " + attr) + globals()[attr] = getattr(module, attr) + return True + + +#_import_module('_transformations') + +if __name__ == "__main__": + import doctest + import random # used in doctests + numpy.set_printoptions(suppress=True, precision=5) + doctest.testmod() + diff --git a/src/caffe/solver.cpp b/src/caffe/solver.cpp index aa1dd9e137e..8140e12631f 100644 --- a/src/caffe/solver.cpp +++ b/src/caffe/solver.cpp @@ -189,10 +189,10 @@ void Solver::Solve(const char* resume_file) { Snapshot(); } - if (param_.test_interval() && iter_ % param_.test_interval() == 0 - && (iter_ > 0 || param_.test_initialization())) { - TestAll(); - } + //if (param_.test_interval() && iter_ % param_.test_interval() == 0 + // && (iter_ > 0 || param_.test_initialization())) { + // TestAll(); + //} const bool display = param_.display() && iter_ % param_.display() == 0; net_->set_debug_info(display && param_.debug_info()); @@ -279,12 +279,19 @@ void Solver::Solve(const char* resume_file) { float x_adj = (qx*grid_dim + grid_dim / 2) / scaling; float y_adj = (qy*grid_dim + grid_dim / 2) / scaling; + //std::cout<<*(bb_start+(((n*64+z)*quad_height+qy)*quad_width+qx))<<" "; + //std::cout<<*(bb_start+(((n*64+z+16)*quad_height+qy)*quad_width+qx))<<" "; + //std::cout<<*(bb_start+(((n*64+z+32)*quad_height+qy)*quad_width+qx))<<" "; + //std::cout<<*(bb_start+(((n*64+z+48)*quad_height+qy)*quad_width+qx))<::Solve(const char* resume_file) { net_->Forward(bottom_vec, &loss); LOG(INFO) << "Iteration " << iter_ << ", loss = " << loss; } - if (param_.test_interval() && iter_ % param_.test_interval() == 0) { - TestAll(); - } + //if (param_.test_interval() && iter_ % param_.test_interval() == 0) { + // TestAll(); + //} LOG(INFO) << "Optimization Done."; }