shen/Happy-Reconstruction
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add core files

Browse Source
This commit is contained in:
Tang1705
2020-05-05 23:30:52 +08:00
parent e53ea3e6b7
commit edcc3e250c
82 changed files with 12139 additions and 308 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1833
Classes/API.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

259
Classes/API.h Normal file
View File

@@ -0,0 +1,259 @@
/*
* API.h
*
* This module provides C callable APIs for each of the command supported by LightCrafter4500 platform and detailed in the programmer's guide.
*
* Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/
* ALL RIGHTS RESERVED
*
*/
#ifndef API_H
#define API_H
/* Bit masks. */
#define BIT0 0x01
#define BIT1 0x02
#define BIT2 0x04
#define BIT3 0x08
#define BIT4 0x10
#define BIT5 0x20
#define BIT6 0x40
#define BIT7 0x80
#define BIT8 0x0100
#define BIT9 0x0200
#define BIT10 0x0400
#define BIT11 0x0800
#define BIT12 0x1000
#define BIT13 0x2000
#define BIT14 0x4000
#define BIT15 0x8000
#define BIT16 0x00010000
#define BIT17 0x00020000
#define BIT18 0x00040000
#define BIT19 0x00080000
#define BIT20 0x00100000
#define BIT21 0x00200000
#define BIT22 0x00400000
#define BIT23 0x00800000
#define BIT24 0x01000000
#define BIT25 0x02000000
#define BIT26 0x04000000
#define BIT27 0x08000000
#define BIT28 0x10000000
#define BIT29 0x20000000
#define BIT30 0x40000000
#define BIT31 0x80000000
#define STAT_BIT_FLASH_BUSY BIT3
#define HID_MESSAGE_MAX_SIZE 512
typedef struct _hidmessageStruct
{
struct _hidhead
{
struct _packetcontrolStruct
{
unsigned char dest :3; /* 0 - ProjCtrl; 1 - RFC; 7 - Debugmsg */
unsigned char reserved :2;
unsigned char nack :1; /* Command Handler Error */
unsigned char reply :1; /* Host wants a reply from device */
unsigned char rw :1; /* Write = 0; Read = 1 */
}flags;
unsigned char seq;
unsigned short length;
}head;
union
{
unsigned short cmd;
unsigned char data[HID_MESSAGE_MAX_SIZE];
}text;
}hidMessageStruct;
typedef struct _readCmdData
{
unsigned char CMD2;
unsigned char CMD3;
unsigned short len;
}CmdFormat;
typedef struct _rectangle
{
unsigned short firstPixel;
unsigned short firstLine;
unsigned short pixelsPerLine;
unsigned short linesPerFrame;
}rectangle;
typedef enum
{
SOURCE_SEL,
PIXEL_FORMAT,
CLK_SEL,
CHANNEL_SWAP,
FPD_MODE,
CURTAIN_COLOR,
POWER_CONTROL,
FLIP_LONG,
FLIP_SHORT,
TPG_SEL,
PWM_INVERT,
LED_ENABLE,
GET_VERSION,
SW_RESET,
DMD_PARK,
BUFFER_FREEZE,
STATUS_HW,
STATUS_SYS,
STATUS_MAIN,
CSC_DATA,
GAMMA_CTL,
BC_CTL,
PWM_ENABLE,
PWM_SETUP,
PWM_CAPTURE_CONFIG,
GPIO_CONFIG,
LED_CURRENT,
DISP_CONFIG,
TEMP_CONFIG,
TEMP_READ,
MEM_CONTROL,
I2C_CONTROL,
LUT_VALID,
DISP_MODE,
TRIG_OUT1_CTL,
TRIG_OUT2_CTL,
RED_STROBE_DLY,
GRN_STROBE_DLY,
BLU_STROBE_DLY,
PAT_DISP_MODE,
PAT_TRIG_MODE,
PAT_START_STOP,
BUFFER_SWAP,
BUFFER_WR_DISABLE,
CURRENT_RD_BUFFER,
PAT_EXPO_PRD,
INVERT_DATA,
PAT_CONFIG,
MBOX_ADDRESS,
MBOX_CONTROL,
MBOX_DATA,
TRIG_IN1_DELAY,
TRIG_IN2_CONTROL,
SPLASH_LOAD,
SPLASH_LOAD_TIMING,
GPCLK_CONFIG,
PULSE_GPIO_23,
ENABLE_LCR_DEBUG,
TPG_COLOR,
PWM_CAPTURE_READ,
PROG_MODE,
BL_STATUS,
BL_SPL_MODE,
BL_GET_MANID,
BL_GET_DEVID,
BL_GET_CHKSUM,
BL_SET_SECTADDR,
BL_SECT_ERASE,
BL_SET_DNLDSIZE,
BL_DNLD_DATA,
BL_FLASH_TYPE,
BL_CALC_CHKSUM,
BL_PROG_MODE,
}LCR_CMD;
int LCR_SetInputSource(unsigned int source, unsigned int portWidth);
int LCR_GetInputSource(unsigned int *pSource, unsigned int *portWidth);
int LCR_SetPixelFormat(unsigned int format);
int LCR_GetPixelFormat(unsigned int *pFormat);
int LCR_SetPortClock(unsigned int clock);
int LCR_GetPortClock(unsigned int *pClock);
int LCR_SetDataChannelSwap(unsigned int port, unsigned int swap);
int LCR_GetDataChannelSwap(unsigned int *pPort, unsigned int *pSwap);
int LCR_SetFPD_Mode_Field(unsigned int PixelMappingMode, bool SwapPolarity, unsigned int FieldSignalSelect);
int LCR_GetFPD_Mode_Field(unsigned int *pPixelMappingMode, bool *pSwapPolarity, unsigned int *pFieldSignalSelect);
int LCR_SetPowerMode(bool);
int LCR_SetLongAxisImageFlip(bool);
bool LCR_GetLongAxisImageFlip();
int LCR_SetShortAxisImageFlip(bool);
bool LCR_GetShortAxisImageFlip();
int LCR_SetTPGSelect(unsigned int pattern);
int LCR_GetTPGSelect(unsigned int *pPattern);
int LCR_SetPWMInvert(bool invert);
int LCR_GetPWMInvert(bool *inverted);
int LCR_SetLedEnables(bool SeqCtrl, bool Red, bool Green, bool Blue);
int LCR_GetLedEnables(bool *pSeqCtrl, bool *pRed, bool *pGreen, bool *pBlue);
int LCR_GetVersion(unsigned int *pApp_ver, unsigned int *pAPI_ver, unsigned int *pSWConfig_ver, unsigned int *pSeqConfig_ver);
int LCR_SoftwareReset(void);
int LCR_GetStatus(unsigned char *pHWStatus, unsigned char *pSysStatus, unsigned char *pMainStatus);
int LCR_SetPWMEnable(unsigned int channel, bool Enable);
int LCR_GetPWMEnable(unsigned int channel, bool *pEnable);
int LCR_SetPWMConfig(unsigned int channel, unsigned int pulsePeriod, unsigned int dutyCycle);
int LCR_GetPWMConfig(unsigned int channel, unsigned int *pPulsePeriod, unsigned int *pDutyCycle);
int LCR_SetPWMCaptureConfig(unsigned int channel, bool enable, unsigned int sampleRate);
int LCR_GetPWMCaptureConfig(unsigned int channel, bool *pEnabled, unsigned int *pSampleRate);
int LCR_SetGPIOConfig(unsigned int pinNum, bool enAltFunc, bool altFunc1, bool dirOutput, bool outTypeOpenDrain, bool pinState);
int LCR_GetGPIOConfig(unsigned int pinNum, bool *pEnAltFunc, bool *pAltFunc1, bool *pDirOutput, bool *pOutTypeOpenDrain, bool *pState);
int LCR_GetLedCurrents(unsigned char *pRed, unsigned char *pGreen, unsigned char *pBlue);
int LCR_SetLedCurrents(unsigned char RedCurrent, unsigned char GreenCurrent, unsigned char BlueCurrent);
int LCR_SetDisplay(rectangle croppedArea, rectangle displayArea);
int LCR_GetDisplay(rectangle *pCroppedArea, rectangle *pDisplayArea);
int LCR_MemRead(unsigned int addr, unsigned int *readWord);
int LCR_MemWrite(unsigned int addr, unsigned int data);
int LCR_ValidatePatLutData(unsigned int *pStatus);
int LCR_SetPatternDisplayMode(bool external);
int LCR_GetPatternDisplayMode(bool *external);
int LCR_SetTrigOutConfig(unsigned int trigOutNum, bool invert, unsigned int rising, unsigned int falling);
int LCR_GetTrigOutConfig(unsigned int trigOutNum, bool *pInvert,unsigned int *pRising, unsigned int *pFalling);
int LCR_SetRedLEDStrobeDelay(unsigned char rising, unsigned char falling);
int LCR_SetGreenLEDStrobeDelay(unsigned char rising, unsigned char falling);
int LCR_SetBlueLEDStrobeDelay(unsigned char rising, unsigned char falling);
int LCR_GetRedLEDStrobeDelay(unsigned char *, unsigned char *);
int LCR_GetGreenLEDStrobeDelay(unsigned char *, unsigned char *);
int LCR_GetBlueLEDStrobeDelay(unsigned char *, unsigned char *);
int LCR_SetProgrammingMode(bool EnterProgMode);
int LCR_ExitProgrammingMode(void);
int LCR_GetProgrammingMode(bool *ProgMode);
int LCR_GetFlashManID(unsigned short *manID);
int LCR_GetFlashDevID(unsigned long long *devID);
int LCR_GetBLStatus(unsigned char *BL_Status);
int LCR_BLSpecialMode(unsigned int Mode);
int LCR_SetFlashAddr(unsigned int Addr);
int LCR_FlashSectorErase(void);
int LCR_SetDownloadSize(unsigned int dataLen);
int LCR_DownloadData(unsigned char *pByteArray, unsigned int dataLen);
void LCR_WaitForFlashReady(void);
int LCR_SetFlashType(unsigned char Type);
int LCR_CalculateFlashChecksum(void);
int LCR_GetFlashChecksum(unsigned int*checksum);
int LCR_SetMode(bool SLmode);
int LCR_GetMode(bool *pMode);
int LCR_LoadSplash(unsigned int index);
int LCR_GetSplashIndex(unsigned int *pIndex);
int LCR_SetTPGColor(unsigned short redFG, unsigned short greenFG, unsigned short blueFG, unsigned short redBG, unsigned short greenBG, unsigned short blueBG);
int LCR_GetTPGColor(unsigned short *pRedFG, unsigned short *pGreenFG, unsigned short *pBlueFG, unsigned short *pRedBG, unsigned short *pGreenBG, unsigned short *pBlueBG);
int LCR_ClearPatLut(void);
int LCR_AddToPatLut(int TrigType, int PatNum,int BitDepth,int LEDSelect,bool InvertPat, bool InsertBlack,bool BufSwap, bool trigOutPrev);
int LCR_GetPatLutItem(int index, int *pTrigType, int *pPatNum,int *pBitDepth,int *pLEDSelect,bool *pInvertPat, bool *pInsertBlack,bool *pBufSwap, bool *pTrigOutPrev);
int LCR_SendPatLut(void);
int LCR_SendSplashLut(unsigned char *lutEntries, unsigned int numEntries);
int LCR_GetPatLut(int numEntries);
int LCR_GetSplashLut(unsigned char *pLut, int numEntries);
int LCR_SetPatternTriggerMode(bool);
int LCR_GetPatternTriggerMode(bool *);
int LCR_PatternDisplay(int Action);
int LCR_SetPatternConfig(unsigned int numLutEntries, bool repeat, unsigned int numPatsForTrigOut2, unsigned int numSplash);
int LCR_GetPatternConfig(unsigned int *pNumLutEntries, bool *pRepeat, unsigned int *pNumPatsForTrigOut2, unsigned int *pNumSplash);
int LCR_SetExpsosure_FramePeriod(unsigned int exposurePeriod, unsigned int framePeriod);
int LCR_GetExposure_FramePeriod(unsigned int *pExposure, unsigned int *pFramePeriod);
int LCR_SetTrigIn1Delay(unsigned int Delay);
int LCR_GetTrigIn1Delay(unsigned int *pDelay);
int LCR_SetInvertData(bool invert);
int LCR_PWMCaptureRead(unsigned int channel, unsigned int *pLowPeriod, unsigned int *pHighPeriod);
int LCR_SetGeneralPurposeClockOutFreq(unsigned int clkId, bool enable, unsigned int clkDivider);
int LCR_GetGeneralPurposeClockOutFreq(unsigned int clkId, bool *pEnabled, unsigned int *pClkDivider);
int LCR_MeasureSplashLoadTiming(unsigned int startIndex, unsigned int numSplash);
int LCR_ReadSplashLoadTiming(unsigned int *pTimingData);
#endif // API_H

179
Classes/CalibrationData.cpp Normal file
View File

@@ -0,0 +1,179 @@
#include "CalibrationData.h"
CalibrationData::CalibrationData() : Kc(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), kc(0.0), cam_error(0.0),
Kp(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), kp(0.0), proj_error(0.0),
Rp(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), Tp(0.0), stereo_error(0.0),
frameWidth(1280), frameHeight(1024), screenResX(0), screenResY(0)
{
}
CalibrationData::CalibrationData(Matx33d _Kc, Vec<double, 5> _kc, double _cam_error, Matx33d _Kp, Vec<double, 5> _kp,
double _proj_error, Matx33d _Rp, Vec3f _Tp, double _stereo_error) :
Kc(_Kc), kc(_kc), cam_error(_cam_error), Kp(_Kp), kp(_kp), proj_error(_proj_error), Rp(_Rp), Tp(_Tp),
stereo_error(_stereo_error)
{
auto cArg = CameraArguments::getInstance(cvtools::Matx33dToMat(Rp), cvtools::Vec3fToMat(Tp), cvtools::Matx33dToMat(_Kc),
cvtools::Matx33dToMat(_Kp));
}
bool CalibrationData::load(const QString& filename)
{
QFileInfo info(filename);
// QString type = info.suffix();
if (info.exists() && info.suffix() == "xml")
return loadXML(filename);
else
{
std::cerr << "CalibrationData error: no such .xml file: " << filename.toStdString() << std::endl;
return false;
}
}
bool CalibrationData::save(const QString& filename)
{
QFileInfo info(filename);
QString type = info.suffix();
if (type == "xml")
{
return saveXML(filename);
}
else if (type == "slcalib")
{
return saveSLCALIB(filename);
}
else if (type == "m")
{
return saveMatlab(filename);
}
else
{
std::cerr << "CalibrationData error save: unknown file extension: " << type.toStdString() << std::endl;
return false;
}
return false;
}
bool CalibrationData::loadXML(const QString& filename)
{
FileStorage fs(filename.toStdString(), FileStorage::READ); //
if (!fs.isOpened())
{
std::cerr << "CalibrationData error: could not open file " << filename.toStdString() << std::endl;
return false;
}
Mat temp;
fs["Kc"] >> temp;
Kc = temp;
fs["kc"] >> temp;
kc = temp;
fs["Kp"] >> temp;
Kp = temp;
fs["kp"] >> temp;
kp = temp;
fs["Rp"] >> temp;
Rp = temp;
fs["Tp"] >> temp;
Tp = temp;
fs["cam_error"] >> cam_error;
fs["proj_error"] >> proj_error;
fs["stereo_error"] >> stereo_error;
fs["frameWidth"] >> frameWidth;
fs["frameHeight"] >> frameHeight;
fs["screenResX"] >> screenResX;
fs["screenResY"] >> screenResY;
fs.release();
return true;
}
bool CalibrationData::saveSLCALIB(const QString& filename)
{
FILE* fp = fopen(qPrintable(filename), "w");
if (!fp)
return false;
fprintf(fp, "#V1.0 SLStudio calibration\n");
fprintf(fp, "#Calibration time: %s\n\n", calibrationDateTime.c_str());
fprintf(fp, "Kc\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Kc(0, 0), Kc(0, 1), Kc(0, 2), Kc(1, 0), Kc(1, 1), Kc(1, 2),
Kc(2, 0), Kc(2, 1), Kc(2, 2));
fprintf(fp, "kc\n%f %f %f %f %f\n\n", kc(0), kc(1), kc(2), kc(3), kc(4));
fprintf(fp, "Kp\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Kp(0, 0), Kp(0, 1), Kp(0, 2), Kp(1, 0), Kp(1, 1), Kp(1, 2),
Kp(2, 0), Kp(2, 1), Kp(2, 2));
fprintf(fp, "kp\n%f %f %f %f %f\n\n", kp(0), kp(1), kp(2), kp(3), kp(4));
fprintf(fp, "Rp\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Rp(0, 0), Rp(0, 1), Rp(0, 2), Rp(1, 0), Rp(1, 1), Rp(1, 2),
Rp(2, 0), Rp(2, 1), Rp(2, 2));
fprintf(fp, "Tp\n%f %f %f\n\n", Tp(0), Tp(1), Tp(2));
fprintf(fp, "cam_error: %f\n\n", cam_error);
fprintf(fp, "proj_error: %f\n\n", proj_error);
fprintf(fp, "stereo_error: %f\n\n", stereo_error);
fclose(fp);
return true;
}
bool CalibrationData::saveXML(const QString& filename)
{
FileStorage fs(filename.toStdString(), FileStorage::WRITE);
if (!fs.isOpened())
return false;
fs << "Kc" << Mat(Kc) << "kc" << Mat(kc)
<< "Kp" << Mat(Kp) << "kp" << Mat(kp)
<< "Rp" << Mat(Rp) << "Tp" << Mat(Tp)
<< "cam_error" << cam_error
<< "proj_error" << proj_error
<< "stereo_error" << stereo_error
<< "frameWidth" << frameWidth
<< "frameHeight" << frameHeight
<< "screenResX" << screenResX
<< "screenResY" << screenResY;
fs.release();
return true;
}
bool CalibrationData::saveMatlab(const QString& filename)
{
std::ofstream file(qPrintable(filename));
if (!file)
return false;
file << "%%SLStudio calibration" << std::endl;
file << "Kc = " << Kc << ";" << std::endl;
file << "kc = " << kc << ";" << std::endl;
file << "Kp = " << Kp << ";" << std::endl;
file << "kp = " << kp << ";" << std::endl;
file << "Rp = " << Rp << ";" << std::endl;
file << "Tp = " << Tp << ";" << std::endl;
file.close();
return true;
}
void CalibrationData::print()
{
std::cout << std::setw(5) << std::setprecision(4)
<< "========================================\n"
<< "Camera Calibration: \n"
<< "- cam_error:\n" << cam_error << "\n"
<< "- Kc:\n" << Kc << "\n"
<< "- kc:\n" << kc << "\n"
<< "Projector Calibration: " << "\n"
<< "- proj_error: \n" << proj_error << "\n"
<< "- Kp: \n" << Kp << "\n"
<< "- kp: \n" << kp << "\n"
<< "Stereo Calibration: \n"
<< "- stereo_error:\n" << stereo_error << "\n"
<< "- Rp:\n" << Rp << "\n"
<< "- Tp:\n" << Tp << std::endl;
}

49
Classes/CalibrationData.h Normal file
View File

@@ -0,0 +1,49 @@
#ifndef CALIBRATIONDATA_H
#define CALIBRATIONDATA_H
#include <QString>
#include <opencv2/core/core.hpp>
#include <QFileInfo>
#include <QDateTime>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <opencv2/calib3d/calib3d.hpp>
#include <qstring.h>
#include "cvtools.h"
#include "CameraArguments.h"
using namespace cv;
class CalibrationData{
public:
CalibrationData();
CalibrationData(cv::Matx33d _Kc, cv::Vec<double, 5> _kc, double _cam_error, cv::Matx33d _Kp, cv::Vec<double, 5> _kp,
double _proj_error, cv::Matx33d _Rp, cv::Vec3f _Tp, double _stereo_error);
bool load(const QString& filename);
bool save(const QString& filename);
bool loadXML(const QString& filename);
bool saveXML(const QString& filename);
bool saveMatlab(const QString& filename);
bool saveSLCALIB(const QString& filename);
void print();
Matx33d Kc; // Intrinsic camera matrix
Vec<double , 5> kc; // Camera distortion coefficients
double cam_error;
Matx33d Kp; // Intrinsic projector matrix
Vec<double , 5> kp; // Projector distortion coefficients
double proj_error;
Matx33d Rp; // Extrinsic camera rotation matrix
Vec3f Tp; // Extrinsic camera rotation matrix
double stereo_error;
int frameWidth , frameHeight;
int screenResX , screenResY;
std::string calibrationDateTime;
};
#endif

338
Classes/Calibrator.cpp Normal file
View File

@@ -0,0 +1,338 @@
#include "Calibrator.h"
#include "structured_light.h"
#include "CalibrationData.h"
#include <fstream>
#include <QtCore/QList>
Calibrator::Calibrator()
{
}
Calibrator::~Calibrator()
{
}
void Calibrator::addFrameSequence(std::vector<cv::Mat> &frameSeq)
{
int n = frameSeqs.size();
frameSeqs.resize(n+1);
std::vector<cv::Mat> &frame = frameSeqs[n];
for(int i = 0; i < frameSeq.size(); i++)
{
frame.push_back(frameSeq[i]);
}
std::cout <<"Calibrator::addFrameSequence : frame.size(): "<< frame.size() << std::endl;
}
void Calibrator::reset()
{
frameSeqs.clear();
board_corners.clear();
projector_corners.clear();
pattern_list.clear();
}
void Calibrator::setBoardRows(int rows) {
board_rows = rows;
board_size = cv::Size(board_rows, board_cols);
}
void Calibrator::setBoardCols(int cols) {
board_cols = cols;
board_size = cv::Size(board_rows, board_cols);
}
void Calibrator::setCornerSize(double cornerSize) {
dot_dis = cornerSize;
corner_size = cv::Size2f(cornerSize, cornerSize);
}
void Calibrator::setDotDiameter(double dotDiameter) {
dot_diameter = dotDiameter;
}
void Calibrator::setCalibBoard(unsigned board) {
board_type = board;
}
CalibrationData* Calibrator::calibrate()
{
CalibrationData *res = new CalibrationData();
// detect corners ////////////////////////////////////
for(int i = 0; i < frameSeqs.size(); i++)
{
std::vector<cv::Point2f> corners = extract_board_corners( frameSeqs[i][0] );
board_corners.push_back(corners);
// std::cout<<i<<" corners: "<<corners.size()<<std::endl;
// for(int j = 0; j < corners.size(); j++) {
// std::cout<<corners[j]<<std::endl;
// }
}
// collect projector correspondences
std::vector<cv::Point2f> pcorners;
for(int i = 0; i < frameSeqs.size(); i++)
{
std::vector<cv::Point2f> const& corners = board_corners[i];
cv::Mat pattern_image;
cv::Mat min_max_image;
if(corners.size()==0)
{
projector_corners.push_back(pcorners);
pattern_list.push_back(pattern_image);
continue;
}
if(!decode_gray_set(i , pattern_image ,min_max_image))
{
projector_corners.push_back(pcorners);
pattern_list.push_back(pattern_image);
continue;
}
pattern_list.push_back(pattern_image);
for (std::vector<cv::Point2f>::const_iterator iter=corners.begin(); iter!=corners.end(); iter++)
{
const cv::Point2f & p = *iter;
cv::Point2f q;
//find an homography around p
unsigned WINDOW_SIZE = 30;
std::vector<cv::Point2f> img_points , proj_points;
if (p.x>WINDOW_SIZE && p.y>WINDOW_SIZE && p.x+WINDOW_SIZE<pattern_image.cols && p.y+WINDOW_SIZE<pattern_image.rows)
{
for (unsigned h=p.y-WINDOW_SIZE; h<p.y+WINDOW_SIZE; h++)
{
register const cv::Vec2f * row = pattern_image.ptr<cv::Vec2f>(h);
register const cv::Vec2b * min_max_row = min_max_image.ptr<cv::Vec2b>(h);
//cv::Vec2f * out_row = out_pattern_image.ptr<cv::Vec2f>(h);
for (unsigned w=p.x-WINDOW_SIZE; w<p.x+WINDOW_SIZE; w++)
{
const cv::Vec2f & pattern = row[w];
const cv::Vec2b & min_max = min_max_row[w];
//cv::Vec2f & out_pattern = out_row[w];
if (std::isnan(pattern[0])>0 || std::isnan(pattern[1])>0)
{
continue;
}
if ((min_max[1]-min_max[0]) < static_cast<int>(threshold))
{ //apply threshold and skip
continue;
}
img_points.push_back(cv::Point2f(w, h));
proj_points.push_back(cv::Point2f(pattern));
//out_pattern = pattern;
}
}
cv::Mat H = cv::findHomography(img_points , proj_points, cv::RANSAC);
// std::cout << " H:\n" << H << std::endl;
cv::Point3d Q = cv::Point3d(cv::Mat(H*cv::Mat(cv::Point3d(p.x , p.y , 1.0))));
q = cv::Point2f(Q.x/Q.z ,Q.y/Q.z);
// jiuzheng
// q.y += 118;
pcorners.push_back(q);
}
else {
pcorners.clear();
break;
}
}
std::cout<<i<<" pcorners: "<<pcorners.size()<<std::endl;
for(int j = 0; j < pcorners.size(); j++) {
std::cout<<pcorners[j]<<std::endl;
}
projector_corners.push_back(pcorners);
pcorners.clear();
}
//generate world object coordinates
unsigned count = 0;
for(int i = 0; i < frameSeqs.size(); i++)
{
if(board_corners[i].size() && projector_corners[i].size())
{
count++;
}
}
std::vector<cv::Point3f> world_corners;
for (int h=0; h<board_size.height; h++)
{
for (int w=0; w<board_size.width; w++)
{
world_corners.push_back(cv::Point3f(corner_size.width * w, corner_size.height * h , 0.f));
}
}
std::vector<std::vector<cv::Point3f> > objectPoints;
objectPoints.reserve(count);
for (unsigned i=0; i<count; i++)
{
objectPoints.push_back(world_corners);
}
//generate world object coordinates
std::vector<cv::Point3f> world_corners_p;
for (int h=0; h<board_size.height; h++)
{
for (int w=0; w<board_size.width; w++)
{
world_corners_p.push_back(cv::Point3f(corner_size.width * w , corner_size.height * h ,0.f));
}
}
std::vector<std::vector<cv::Point3f> > objectPoints_p;
objectPoints_p.reserve(count);
for (unsigned i=0; i<count; i++)
{
objectPoints_p.push_back(world_corners_p);
}
//collect projector correspondences
projector_corners.resize(count);
pattern_list.resize(count);
int cal_flags = 0
//+ cv::CALIB_FIX_K1
//+ cv::CALIB_FIX_K2
//+ cv::CALIB_ZERO_TANGENT_DIST
+ cv::CALIB_FIX_K3
;
//calibrate the camera ////////////////////////////////////
std::vector<cv::Mat> cam_rvecs , cam_tvecs;
int cam_flags = cal_flags;
cv::Size imageSize = frameSeqs[0][0].size();
res->cam_error = cv::calibrateCamera(objectPoints , board_corners , imageSize , res->Kc , res->kc ,cam_rvecs , cam_tvecs);
std::cout<<"calibrate the camera !"<<std::endl;
//calibrate the projector ////////////////////////////////////
std::vector<cv::Mat> proj_rvecs , proj_tvecs;
int proj_flags = cal_flags;
cv::Size projector_size(912, 1140);
res->proj_error = cv::calibrateCamera(objectPoints_p , projector_corners, projector_size , res->Kp , res->kp , proj_rvecs , proj_tvecs);
std::cout<<"calibrate the projector !"<<std::endl;
//stereo calibration
cv::Mat E, F;
res->stereo_error = cv::stereoCalibrate(objectPoints , board_corners , projector_corners ,
res->Kc , res->kc , res->Kp ,res->kp ,imageSize /*ignored*/ ,
res->Rp , res->Tp , E , F );
// res->stereo_error = cv::stereoCalibrate(objectPoints , board_corners , projector_corners , res->Kc , res->kc , res->Kp ,res->kp ,imageSize /*ignored*/ , res->Rp , res->Tp , E , F ,
// cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS ,50 ,DBL_EPSILON) ,
// cv::CALIB_FIX_INTRINSIC /*cv::CALIB_USE_INTRINSIC_GUESS + cal_flags*/);
std::cout<<"stereo calibration !"<<std::endl;
cv::FileStorage fsc("rtc.xml", cv::FileStorage::WRITE);
cv::FileStorage fsp("rtp.xml", cv::FileStorage::WRITE);
// objectPoints projector_corners board_corners
std::ofstream opout("objectPoints.txt");
std::ofstream pcout("projector_corners.txt");
std::ofstream ccout("board_corners.txt");
for (int i = 0; i < objectPoints.size(); i++) {
fsc << "R" + std::to_string(i) << cv::Mat(cam_rvecs[i]);
fsc << "T" + std::to_string(i) << cv::Mat(cam_tvecs[i]);
fsp << "R" + std::to_string(i) << cv::Mat(proj_rvecs[i]);
fsp << "T" + std::to_string(i) << cv::Mat(proj_tvecs[i]);
for (int j = 0; j < objectPoints[i].size(); j++) {
opout << objectPoints[i][j] << std::endl;
pcout << projector_corners[i][j] << std::endl;
ccout << board_corners[i][j] << std::endl;
}
}
opout.close();
pcout.close();
ccout.close();
fsc.release();
fsp.release();
return res;
}
std::vector<cv::Point2f> Calibrator::extract_board_corners(cv::Mat &gray_image)
{
std::vector<cv::Point2f> corners;
int image_scale = 1;
if (gray_image.rows<1)
{
return corners;
}
cv::Size imageSize = gray_image.size();
if (imageSize.width>1024)
{
image_scale = imageSize.width/1024;
}
cv::Mat small_img;
if (image_scale>1)
{
cv::resize(gray_image , small_img , cv::Size(gray_image.cols/image_scale , gray_image.rows/image_scale));
}
else
{
small_img = gray_image;
}
if(board_type == Chessboard) {
cv::findChessboardCorners(small_img, board_size, corners);
}
else if(board_type == Circular) {
cv::findCirclesGrid(small_img, board_size, corners);
}
if (corners.size())
{
for (std::vector<cv::Point2f>::iterator iter=corners.begin(); iter!=corners.end(); iter++)
{
*iter = image_scale*(*iter);
}
cv::cornerSubPix(gray_image , corners , cv::Size(11 , 11) , cv::Size(-1 , -1) ,
cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30 , 0.1));
}
return corners;
}
bool Calibrator::decode_gray_set(int ind , cv::Mat & pattern_image, cv::Mat & min_max_image)
{
if (ind >= frameSeqs.size())
{ //out of bounds
return false;
}
//estimate direct component
//b = config.value("robust_estimation/b" DEFAULT_B).toFloat();
std::vector<cv::Mat> images;
// QList<unsigned> direct_component_images(QList<unsigned>() << 15 << 16 << 17 << 18 << 35 << 36 << 37 << 38);
int total_images = frameSeqs[ind].size();
int total_patterns = total_images/2 - 1;
const int direct_light_count = 4;
const int direct_light_offset = 4;
QList<unsigned> direct_component_images;
for (unsigned i=0; i<direct_light_count; i++)
{
int index = total_images - total_patterns - direct_light_count - direct_light_offset + i + 1;
direct_component_images.append(index);
direct_component_images.append(index + total_patterns);
}
foreach (unsigned i , direct_component_images)
{
images.push_back(frameSeqs[ind][i]);
}
cv::Mat direct_light = sl::estimate_direct_light(images, b);
//m = config.value("robust_estimation/m" DEFAULT_M).toUInt();
// return sl::decode_pattern(frameSeqs[ind] , pattern_image , min_max_image , sl::RobustDecode|sl::GrayPatternDecode , direct_light, m);
cv::Size projector_size(912, 1140);
bool rv = sl::decode_pattern(frameSeqs[ind], pattern_image, min_max_image, projector_size, sl::RobustDecode|sl::GrayPatternDecode, direct_light, m);
return rv;
}

50
Classes/Calibrator.h Normal file
View File

@@ -0,0 +1,50 @@
#ifndef CALIBRATOR_H
#define CALIBRATOR_H
#include <opencv2/opencv.hpp>
#include <vector>
#include "CalibrationData.h"
class Calibrator
{
public:
Calibrator();
~Calibrator();
void addFrameSequence(std::vector<cv::Mat> &frameSeq);
void reset();
CalibrationData * calibrate();
std::vector<cv::Point2f> extract_board_corners(cv::Mat &gray_image);
bool decode_gray_set(int i , cv::Mat & pattern_image, cv::Mat & min_max_image);
void setBoardRows(int rows);
void setBoardCols(int cols);
void setCornerSize(double cornerSize);
void setDotDiameter(double dotDiameter);
void setCalibBoard(unsigned board);
const static unsigned Chessboard = 0x00;
const static unsigned Circular = 0x01;
protected:
std::vector< std::vector<cv::Mat> > frameSeqs;
std::vector< std::vector<cv::Point2f> > board_corners;
std::vector< std::vector<cv::Point2f> > projector_corners;
std::vector<cv::Mat> pattern_list;
cv::Size2f corner_size = cv::Size2f(20.f , 20.f);
cv::Size board_size = cv::Size(8 , 6);
int board_rows = 8;
int board_cols = 6;
double dot_dis = 20.0;
double dot_diameter = 7.5;
unsigned board_type = Chessboard;
unsigned threshold = 10;
float b = 0.5;
unsigned m = 5;
};
#endif // CALIBRATOR_H

28
Classes/Camera.cpp Normal file
View File

@@ -0,0 +1,28 @@
#include "Camera.h"
#include <iostream>
#include "CameraPointGrey.h"
// Global camera enumerator
std::vector< std::vector<CameraInfo> > Camera::GetInterfaceCameraList(){
std::vector< std::vector<CameraInfo> > ret;
#ifdef WITH_CAMERAPOINTGREY
std::vector<CameraInfo> ptgreycameras = CameraPointGrey::getCameraList();
ret.push_back(ptgreycameras);
#endif
return ret;
}
// Camera factory
Camera* Camera::NewCamera(unsigned int interfaceNum , unsigned int camNum , CameraTriggerMode triggerMode){
std::cout << "aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa" << std::endl;
if(interfaceNum == 0)
return new CameraPointGrey(camNum , triggerMode);
return (Camera*)NULL;
}

60
Classes/Camera.h Normal file
View File

@@ -0,0 +1,60 @@
#ifndef CAMERA_H
#define CAMERA_H
#include <iostream>
#include <vector>
struct CameraFrame {
unsigned char *memory;
unsigned int width;
unsigned int height;
unsigned int sizeBytes;
unsigned int timeStamp;
CameraFrame(): memory(NULL), width(0), height(0), sizeBytes(0), timeStamp(0){}
};
struct CameraSettings {
float gain;
float shutter; // [ms]
CameraSettings(): gain(0.0), shutter(0.0) {}
};
struct CameraInfo {
std::string vendor;
std::string model;
unsigned int busID;
};
enum CameraTriggerMode {
triggerModeHardware ,
triggerModeSoftware
};
// Camera factory methods and abstract base class for camera implementations
class Camera {
public:
// Static "camera factory" methods
static std::vector< std::vector<CameraInfo> > GetInterfaceCameraList();
static Camera* NewCamera(unsigned int interfaceNum, unsigned int camNum, CameraTriggerMode triggerMode);
// Interface function
Camera(CameraTriggerMode _triggerMode) : capturing(false), connecting(false), triggerMode(_triggerMode){}
virtual void startCapture() = 0;
bool isCapturing(){return capturing;}
bool isConnecting(){return connecting;}
virtual void stopCapture() = 0;
virtual CameraFrame getFrame() = 0;
virtual size_t getFrameSizeBytes() = 0;
virtual size_t getFrameWidth() = 0;
virtual size_t getFrameHeight() = 0;
virtual CameraSettings getCameraSettings() = 0;
virtual void setCameraSettings(CameraSettings) = 0;
virtual ~Camera(){ }
protected:
bool capturing;
bool connecting;
CameraTriggerMode triggerMode;
};
#endif

4
Classes/CameraArguments.cpp
View File

@@ -17,8 +17,8 @@ CameraArguments::CameraArguments(cv::Mat r, cv::Mat t, cv::Mat kc, cv::Mat kp)
kc1 = kc;
kp2 = kp;
cv::Mat tmp;
hconcat(cv::Mat::eye(3, 3,CV_32FC1),
cv::Mat::zeros(cv::Size(5, 5), CV_32FC1), tmp);
hconcat(cv::Mat::eye(3, 3, CV_32FC1),
cv::Mat::zeros(cv::Size(1, 3), CV_32FC1), tmp);
hc1 = kc1 * tmp;
hconcat(r12, t12.t(), tmp);
hp2 = kp2 * tmp;

306
Classes/CameraPointGrey.cpp Normal file
View File

@@ -0,0 +1,306 @@
#include "CameraPointGrey.h"
#include <cstring>
void PrintError(FlyCapture2::Error error){
error.PrintErrorTrace();
}
vector<CameraInfo> CameraPointGrey::getCameraList(){
FlyCapture2::Error error;
FlyCapture2::BusManager busManager;
unsigned int numCameras;
error = busManager.GetNumOfCameras(&numCameras);
vector<CameraInfo> ret;
if (error != FlyCapture2::PGRERROR_OK){
PrintError(error);
return ret;
}
for (unsigned int i=0; i < numCameras; i++){
FlyCapture2::PGRGuid guid;
error = busManager.GetCameraFromIndex(i , &guid);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
// Connect to camera
FlyCapture2::Camera cam;
error = cam.Connect(&guid);
if (error != FlyCapture2::PGRERROR_OK)
PrintError( error );
// Get the camera information
FlyCapture2::CameraInfo camInfo;
error = cam.GetCameraInfo(&camInfo);
if (error != FlyCapture2::PGRERROR_OK)
PrintError( error );
CameraInfo camera;
camera.busID = camInfo.nodeNumber;
camera.model = camInfo.modelName;
camera.vendor = "Point Grey Research";
ret.push_back(camera);
}
return ret;
}
CameraPointGrey::CameraPointGrey(unsigned int camNum , CameraTriggerMode triggerMode) : Camera(triggerMode){
FlyCapture2::Error error;
// Connect to camera
FlyCapture2::BusManager busManager;
FlyCapture2::PGRGuid camGuid;
busManager.GetCameraFromIndex(camNum , &camGuid);
error = cam.Connect(&camGuid);
if (error != FlyCapture2::PGRERROR_OK) {
PrintError(error);
connecting = false;
return ;
}
FlyCapture2::Format7ImageSettings format7Settings;
format7Settings.mode = FlyCapture2::MODE_0;
format7Settings.pixelFormat = FlyCapture2::PIXEL_FORMAT_RGB8;
format7Settings.width = 1280;
format7Settings.height = 1024;
format7Settings.offsetX = 0;
format7Settings.offsetY = 0;
// Validate and set mode
FlyCapture2::Format7PacketInfo packetInfo;
bool valid;
error = cam.ValidateFormat7Settings(&format7Settings , &valid ,&packetInfo);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
// packetsize configures maximum frame rate
error = cam.SetFormat7Configuration(&format7Settings, packetInfo.recommendedBytesPerPacket);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
// Turn off gamma
FlyCapture2::Property property;
property.type = FlyCapture2::AUTO_EXPOSURE;
property.onOff = false;
error = cam.SetProperty(&property);
if (error != FlyCapture2::PGRERROR_OK){
PrintError(error);
connecting = false;
return ;
}
property.type = FlyCapture2::GAMMA;
property.onOff = true;
property.absControl = true;
property.absValue = 1.0;
error = cam.SetProperty(&property);
if (error != FlyCapture2::PGRERROR_OK){
PrintError(error);
connecting = false;
return ;
}
// Get the camera information
FlyCapture2::CameraInfo camInfo;
error = cam.GetCameraInfo(&camInfo);
if (error != FlyCapture2::PGRERROR_OK){
PrintError(error);
connecting = false;
return ;
}
std::cout << camInfo.vendorName << " " << camInfo.modelName << " " << camInfo.serialNumber << std::endl;
connecting = true;
// Set reasonable default settings
CameraSettings settings;
//settings.shutter = 8.33;
settings.shutter = 33.33;
settings.gain = 0.0;
this->setCameraSettings(settings);
return;
}
CameraSettings CameraPointGrey::getCameraSettings(){
FlyCapture2::Property property;
// Get settings:
CameraSettings settings;
property.type = FlyCapture2::SHUTTER;
cam.GetProperty(&property);
settings.shutter = property.absValue;
property.type = FlyCapture2::GAIN;
cam.GetProperty(&property);
settings.gain = property.absValue;
return settings;
}
void CameraPointGrey::setCameraSettings(CameraSettings settings){
/*
FlyCapture2::Property property;
property.onOff = true;
property.absControl = true;
property.type = FlyCapture2::SHUTTER;
property.absValue = settings.shutter;
cam.SetProperty(&property);
property.type = FlyCapture2::GAIN;
property.absValue = settings.gain;
cam.SetProperty(&property);
*/
}
void CameraPointGrey::startCapture(){
FlyCapture2::Error error;
CameraSettings settings = this->getCameraSettings();
std::cout << "\tShutter: " << settings.shutter << "ms" << std::endl;
std::cout << "\tGain: " << settings.gain << "dB" << std::endl;
if(triggerMode == triggerModeHardware){
// Configure for hardware trigger
FlyCapture2::TriggerMode triggerMode;
triggerMode.onOff = true;
triggerMode.polarity = 0;
triggerMode.source = 0;
triggerMode.mode = 14;
error = cam.SetTriggerMode(&triggerMode);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
error = cam.StartCapture();
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
} else if(triggerMode == triggerModeSoftware){
// Configure software trigger
FlyCapture2::TriggerMode triggerMode;
triggerMode.onOff = true;
triggerMode.polarity = 0;
triggerMode.source = 7; // software
triggerMode.mode = 0;
error = cam.SetTriggerMode(&triggerMode);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
}
// Set the trigger timeout to 1000 ms
FlyCapture2::FC2Config config;
config.grabTimeout = 1000;
error = cam.SetConfiguration(&config);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
error = cam.StartCapture();
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
capturing = true;
}
void CameraPointGrey::stopCapture(){
FlyCapture2::Error error = cam.StopCapture();
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
capturing = false;
}
CameraFrame CameraPointGrey::getFrame(){
FlyCapture2::Error error;
if(triggerMode == triggerModeSoftware){
// Fire software trigger
// broadcasting not supported on some platforms
cam.FireSoftwareTrigger(false);
}
// Retrieve the image
FlyCapture2::Image rawImage;
error = cam.RetrieveBuffer(&rawImage);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
CameraFrame frame;
frame.timeStamp = rawImage.GetTimeStamp().cycleCount;
frame.height = rawImage.GetRows();
frame.width = rawImage.GetCols();
frame.memory = rawImage.GetData();
return frame;
}
size_t CameraPointGrey::getFrameSizeBytes(){
FlyCapture2::Format7ImageSettings format7Settings;
unsigned int dummy1;
float dummy2;
FlyCapture2::Error error = cam.GetFormat7Configuration(&format7Settings , &dummy1, &dummy2);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
return format7Settings.width*format7Settings.height;
}
size_t CameraPointGrey::getFrameWidth(){
FlyCapture2::Format7ImageSettings format7Settings;
unsigned int dummy1;
float dummy2;
FlyCapture2::Error error = cam.GetFormat7Configuration(&format7Settings, &dummy1 , &dummy2);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
return format7Settings.width;
}
size_t CameraPointGrey::getFrameHeight(){
FlyCapture2::Format7ImageSettings format7Settings;
unsigned int dummy1;
float dummy2;
FlyCapture2::Error error = cam.GetFormat7Configuration(&format7Settings , &dummy1 , &dummy2);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
return format7Settings.height;
}
CameraPointGrey::~CameraPointGrey(){
if(capturing){
// Stop camera transmission
cam.StopCapture();
}
// Gracefulle destruct the camera
cam.Disconnect();
std::cout<<"camera disconnect"<<std::endl;
}

28
Classes/CameraPointGrey.h Normal file
View File

@@ -0,0 +1,28 @@
#ifndef CameraPointGrey_H
#define CameraPointGrey_H
#include "Camera.h"
#include <FlyCapture2.h>
using namespace std;
class CameraPointGrey : public Camera {
public:
// Static methods
static vector<CameraInfo> getCameraList();
// Interface function
CameraPointGrey(unsigned int camNum , CameraTriggerMode triggerMode);
CameraSettings getCameraSettings();
void setCameraSettings(CameraSettings);
void startCapture();
void stopCapture();
CameraFrame getFrame();
size_t getFrameSizeBytes();
size_t getFrameWidth();
size_t getFrameHeight();
~CameraPointGrey();
private:
FlyCapture2::Camera cam;
};
#endif

108
Classes/Common.h Normal file
View File

@@ -0,0 +1,108 @@
/*
* Common.h
*
* This module provides the common defines used by all the modules.
*
* Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/
* ALL RIGHTS RESERVED
*
*/
#ifndef COMMON_H_
#define COMMON_H_
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#ifdef __cplusplus
extern "C" {
#endif
#define TRUE 1
#define FALSE 0
typedef enum
{
IMAGE_PIX_FORMAT_RGB32,
IMAGE_PIX_FORMAT_GREY8,
IMAGE_PIX_FORMAT_GREY10,
IMAGE_PIX_FORMAT_UYVY16,
IMAGE_PIX_FORMAT_RGB16,
IMAGE_PIX_FORMAT_SBGGR,
IMAGE_PIX_FORMAT_RGB24,
} ImagePixFormat_t;
typedef struct
{
unsigned char *Buffer;
unsigned Width;
unsigned Height;
unsigned LineWidth;
ImagePixFormat_t PixFormat;
}Image_t;
typedef int BOOL;
typedef unsigned uint32;
typedef unsigned char uint8;
typedef unsigned short uint16;
typedef uint16 PatternCount_t;
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define ALIGN_BYTES_PREV(x, b) ((x) & ~(uint32)((b) - 1))
#define ALIGN_BYTES_NEXT(x, b) (((x) + ((b)-1)) & ~(uint32)((b) - 1))
#define GET_BYTE0(x) ((x) & 0xFF)
#define GET_BYTE1(x) (((x) >> 8) & 0xFF)
#define GET_BYTE2(x) (((x) >> 16) & 0xFF)
#define GET_BYTE3(x) (((x) >> 24) & 0xFF)
#define PARSE_WORD16_LE(Arr) MAKE_WORD16((Arr)[1], (Arr)[0])
#define PARSE_WORD16_BE(Arr) MAKE_WORD16((Arr)[0], (Arr)[1])
#define PARSE_WORD24_LE(Arr) MAKE_WORD32(0, (Arr)[2], (Arr)[1], (Arr)[0])
#define PARSE_WORD24_BE(Arr) MAKE_WORD32(0, (Arr)[0], (Arr)[1], (Arr)[2])
#define PARSE_WORD32_LE(Arr) MAKE_WORD32((Arr)[3], (Arr)[2], (Arr)[1], (Arr)[0])
#define PARSE_WORD32_BE(Arr) MAKE_WORD32((Arr)[0], (Arr)[1], (Arr)[2], (Arr)[3])
#define MAKE_WORD16(b1, b0) (((b1) << 8) | (b0))
#define MAKE_WORD32(b3, b2, b1, b0) (((uint32)(b3)<<24)|((uint32)(b2)<<16)|((uint32)(b1)<<8)|(b0))
#define ARRAY_SIZE(x) (sizeof(x)/sizeof(*x))
#define DIV_ROUND(x, y) (((x)+(y)/2)/(y))
#define DIV_CEIL(x, y) (((x)+(y)-1)/(y))
#define POW_OF_2(x) (1ul << (x))
#define IS_POW_OF_2(x) (((x) & ((x)-1)) == 0)
// Generate bit mask of n bits starting from s bit
#define GEN_BIT_MASK(s, n) (((1ul << (n)) - 1) << (s))
// Merge bits b into a according to mask
#define MERGE_BITS(a, b, mask) ((a) ^ (((a) ^ (b)) & (mask)))
#define STRUCT_CLEAR(s) (memset(&(s), 0, sizeof(s)))
#define BIT_REVERSE(x) CMN_BitRevLUT[(uint8)(x)]
#define STR(x) STR_(x)
#define STR_(x) #x
extern const uint8 CMN_BitRevLUT[];
void PrintArray(char const *Prefix, uint8 const *Array, unsigned Size, BOOL Hex);
uint32 GetImagePixel(Image_t *Image, int x, int y);
uint32 Next2Power(uint32 Value);
unsigned Hex2BinArray(char *HexStr, unsigned Size, uint8 *BinArray);
int TrimString(char const *Input, char *Output);
int WriteTextToFile(char const *File, int Num, char const *Text);
int ReadTextFromFile(char const *File, int Num, char *Text, int Len);
BOOL FileExist(char const *File, int Num);
#ifdef __cplusplus
}
#endif
#endif /* COMMON_H_ */

591
Classes/CoreAlgorithm.cpp
View File

@@ -1,76 +1,111 @@
#include "CoreAlgorithm.h"
#include <unordered_map>
CoreAlgorithm::CoreAlgorithm(std::string path = "../Data/image/reconstruction/tq.png")
CoreAlgorithm::CoreAlgorithm(const std::string& path, CameraArguments* cArgs)
{
image = cv::imread(path, cv::IMREAD_COLOR);
tmp = image.clone(); // 深拷贝
split(image, channel); //b,g,r
cvtColor(image, hsv, COLOR_BGR2HSV);
cArg = CameraArguments::getInstance();
image = imread(path, IMREAD_COLOR);
rows = image.rows;
cols = image.cols;
split(image, rgbChannel); //b,g,r
hsv = image.clone();
cvtColor(image, hsv, COLOR_BGR2HSV, 3);
split(hsv, hsvChannel);
cvtColor(image, lab, COLOR_BGR2Lab);
cArg = cArgs;
}
CoreAlgorithm::~CoreAlgorithm()
= default;
Mat CoreAlgorithm::OSTU(Mat src)
Mat CoreAlgorithm::OtsuAlgThreshold(Mat& src)
{
const auto width = src.cols;
const auto height = src.rows;
int hisData[256] = {0};
for (auto j = 0; j < height; j++)
if (src.channels() != 1)
{
auto* data = src.ptr<uchar>(j);
for (auto i = 0; i < width; i++)
hisData[data[i]]++;
cout << "Please input Gray-src!" << endl;
}
auto t0 = 0;
auto T = 0;
double varValue = 0;
double w0 = 0;
double w1 = 0;
double u0 = 0;
double u1 = 0;
double Histogram[256] = {0};
uchar* data = src.data;
for (auto i = 0; i < 256; i++)
double totalNum = src.rows * src.cols;
for (auto i = 0; i < src.rows; i++)
{
t0 += i * hisData[i];
}
t0 /= width * height;
auto t1 = 0, t2 = 0;
auto num1 = 0, num2 = 0;
auto t = 0;
while (true)
{
for (auto i = 0; i < t0 + 1; i++)
for (auto j = 0; j < src.cols; j++)
{
t1 += i * hisData[i];
num1 += hisData[i];
if (src.at<float>(i, j) != 0) Histogram[data[i * src.step + j]]++;
}
if (num1 == 0) continue;
for (auto i = t0 + 1; i < 256; i++)
{
t2 += i * hisData[i];
num2 += hisData[i];
}
if (num2 == 0) continue;
t = (t1 / num1 + t2 / num2) / 2;
if (t != t0) t0 = t;
else break;
}
Mat dst;
auto minpos = 0, maxpos = 0;
for (auto i = 0; i < 255; i++)
{
if (Histogram[i] != 0)
{
minpos = i;
break;
}
}
threshold(src, dst, t, 255, 0);
for (auto i = 255; i > 0; i--)
{
if (Histogram[i] != 0)
{
maxpos = i;
break;
}
}
for (auto i = minpos; i <= maxpos; i++)
{
w1 = 0;
u1 = 0;
w0 = 0;
u0 = 0;
for (auto j = 0; j <= i; j++)
{
w1 += Histogram[j];
u1 += j * Histogram[j];
}
if (w1 == 0)
{
break;
}
u1 = u1 / w1;
w1 = w1 / totalNum;
for (auto k = i + 1; k < 255; k++)
{
w0 += Histogram[k];
u0 += k * Histogram[k];
}
if (w0 == 0)
{
break;
}
u0 = u0 / w0;
w0 = w0 / totalNum;
auto varValueI = w0 * w1 * (u1 - u0) * (u1 - u0);
if (varValue < varValueI)
{
varValue = varValueI;
T = i;
}
}
// cout << T << endl;
Mat dst = src.clone();
for (auto i = 0; i < src.rows; i++)
for (auto j = 0; j < src.cols; j++)
dst.at<float>(i, j) = src.at<float>(i, j) > T ? 255 : 0;
return dst;
}
vector<int> CoreAlgorithm::Debruijn(int k, int n)
vector<int> CoreAlgorithm::DeBruijn(int k, int n)
{
std::vector<byte> a(k * n, 0);
std::vector<byte> seq;
@@ -118,107 +153,149 @@ vector<int> CoreAlgorithm::Debruijn(int k, int n)
return res;
}
vector<int> CoreAlgorithm::Hsv(int r, int g, int b)
void CoreAlgorithm::Reconstruction(vector<vector<float>> maximas, vector<vector<float>> minimas,
vector<vector<float>> colorLabel, vector<vector<float>> phases, const Mat& Hc1,
Mat Hp2, const double* map)
{
vector<int> h = {0, 0, 0, 0};
h[0] = (2 * r - g - b) / (2 * sqrt(pow((r - g), 2) + (r - b) * (g - b)));
h[1] = (2 * g - r - b) / (2 * sqrt(pow((g - r), 2) + (g - b) * (r - b)));
h[2] = (2 * b - g - r) / (2 * sqrt(pow((b - g), 2) + (b - r) * (g - r)));
h[3] = sqrt(1 - ((r * g + g * b + r * b) / (pow(r, 2) + pow(g, 2) + pow(b, 2)))); // 色彩强度
return h;
}
Mat CoreAlgorithm::Reconstruction(Mat featurePoint, int num, Mat Hc1, Mat Hp2, Mat map)
{
auto numOfCoord = 0;
Mat coordinate = Mat::zeros(cv::Size(num, 3), CV_32FC1);
for (auto column = minX; column < maxX; column++)
for (auto i = 0; i < maximas.size(); i++)
{
Mat rowPosition = Mat::zeros(cv::Size(700, 1), CV_32FC1);
auto counter = 0;
for (auto row = minY; row < maxY; row++)
if (maximas[i].empty())continue;
if (maximas[i].size() < 4)continue;
auto mark = 0;
// double pc = 0;
for (auto j = 0; j < maximas[i].size(); j++)
{
if (featurePoint.at<uchar>(row, column) >= 0)
double position;
if (j < maximas[i].size() - 3)
{
rowPosition.at<uchar>(0, counter) = row;
counter++;
position = map[int(pow(3, 3) * colorLabel[i].at(j) + pow(3, 2) * colorLabel[i].at(j + 1) +
3 * colorLabel[i].at(j + 2) + colorLabel[i].at(j + 3))];
}
if (counter < 4)
else
{
continue;
auto fix = maximas[i].size() - 4;
auto index = j - maximas[i].size() + 4;
position = map[int(pow(3, 3) * colorLabel[i].at(fix) + pow(3, 2) * colorLabel[i].at(fix + 1) +
3 * colorLabel[i].at(fix + 2) + colorLabel[i].at(fix + 3))] + 14.0 * index;
}
for (auto c = 0; c < counter - 3; c++)
Mat matrix = Mat::zeros(cv::Size(3, 3), CV_32FC1);
matrix.row(0) = Hc1(Rect(0, 2, 3, 1)) * (maximas[i][j]) - Hc1(Rect(0, 0, 3, 1));
matrix.row(1) = Hc1(Rect(0, 2, 3, 1)) * (float(i + minX)) - Hc1(Rect(0, 1, 3, 1));
matrix.row(2) = Hp2(Rect(0, 2, 3, 1)) * position - Hp2(Rect(0, 0, 3, 1));
Mat tang = Mat::zeros(cv::Size(3, 1), CV_32FC1);
Mat b = Mat::zeros(cv::Size(1, 3), CV_32FC1);
b.row(0) = Hc1.at<float>(0, 3) - Hc1.at<float>(2, 3) * (maximas[i][j]);
b.row(1) = Hc1.at<float>(1, 3) - Hc1.at<float>(2, 3) * (float(i + minX));
b.row(2) = Hp2.at<float>(0, 3) - Hp2.at<float>(2, 3) * position;
solve(matrix, b, tang);
if (tang.at<float>(2, 0) > 750 && tang.at<float>(2, 0) < 1500)
{
Mat matrix = Mat::zeros(cv::Size(3, 3), CV_32FC1);
matrix.row(0) = Hc1.row(2) * (rowPosition.at<uchar>(0, c) + 1) - Hc1.row(0);
matrix.row(1) = Hc1.row(2) * (column + 1) - Hc1.row(1);
coordinate.push_back(tang.t());
auto p1 = rowPosition.at<uchar>(0, c);
auto p2 = rowPosition.at<uchar>(0, c + 1);
auto p3 = rowPosition.at<uchar>(0, c + 2);
auto p4 = rowPosition.at<uchar>(0, c + 3);
auto id = map.step[0] * featurePoint.at<uchar>(column, p1)
+ map.step[1] * featurePoint.at<uchar>(column, p2)
+ map.step[2] * featurePoint.at<uchar>(column, p3)
+ map.step[3] * featurePoint.at<uchar>(column, p4);
matrix.row(2) = Hp2.row(2) * (*reinterpret_cast<int*>(map.data + id)
- Hp2.row(0));
Mat tang = Mat::zeros(cv::Size(3, 1), CV_32FC1);
Mat tmpMat = Mat::zeros(cv::Size(3, 1), CV_32FC1);
tmpMat.row(0) = Hc1.row(0) - Hc1.row(2) * (rowPosition.at<uchar>(0, c) + 1);
tmpMat.row(1) = Hc1.row(1) - Hc1.row(2) * (column + 1);
tmpMat.row(2) = Hp2.row(0) - Hp2.at<uchar>(2, 3) * (*reinterpret_cast<int*>(map.data + id));
tmpMat = tmpMat.t();
solve(matrix, tmpMat, tang);
if (tang.at<uchar>(2, 0) > 750 && tang.at<uchar>(2, 0) < 1500)
int r = (int)rgbChannel[2].at<uchar>(i + minX, maximas[i][j]),
g = rgbChannel[1].at<uchar>(i + minX, maximas[i][j]),
b = rgbChannel[0].at<uchar>(i + minX, maximas[i][j]);
int rgb = ((int)r << 16 | (int)g << 8 | (int)b);
float frgb = *reinterpret_cast<float*>(&rgb);
color.push_back(frgb);
}
// if (i == 200)cout << maximas[i][j] << "," << 0 << "," << position << endl;
if (phases[i].empty())continue;
auto pi = false;
auto start = minimas[i][0];
if (start > maximas[i][j]) continue;
if (j == 0)
{
for (auto k = mark; k + start < maximas[i][j]; k++)
{
coordinate.row(numOfCoord) = tang.t();
numOfCoord++;
if ((start + k) < maximas[i][j] && phases[i][k] < 0)continue;
if ((start + k) < maximas[i][j] && phases[i][k] > 0)
{
if (maximas[i][j] - (start + k) < 1)
{
continue;
}
mark = k + 1;
}
else if ((start + k) > maximas[i][j]) break;
}
}
for (auto k = mark; k < phases[i].size()-1; k++)
{
mark++;
double newPosition;
if ((start + k) < maximas[i][j] && phases[i][k] < 0) newPosition = position + phases[i][k];
else if ((maximas[i][j] - (start + k)) > 1 && phases[i][k] > 0)
newPosition = position + phases[i][k] - 7;
else if ((start + k) > maximas[i][j] && phases[i][k] > 0)newPosition = position + phases[i][k];
else if (((start + k) - maximas[i][j]) > 1 && phases[i][k] < 0)
newPosition = position + phases[i][k] + 7;
else continue;
matrix.row(0) = Hc1(Rect(0, 2, 3, 1)) * (start + k) - Hc1(Rect(0, 0, 3, 1));
matrix.row(2) = Hp2(Rect(0, 2, 3, 1)) * newPosition - Hp2(Rect(0, 0, 3, 1));
b.row(0) = Hc1.at<float>(0, 3) - Hc1.at<float>(2, 3) * (start + k);
b.row(2) = Hp2.at<float>(0, 3) - Hp2.at<float>(2, 3) * newPosition;
solve(matrix, b, tang);
if (tang.at<float>(2, 0) > 750 && tang.at<float>(2, 0) < 1500)
{
coordinate.push_back(tang.t());
int r = (int)rgbChannel[2].at<uchar>(i + minX, (start + k)),
g = rgbChannel[1].at<uchar>(i + minX, (start + k)),
b = rgbChannel[0].at<uchar>(i + minX, (start + k));
int rgb = ((int)r << 16 | (int)g << 8 | (int)b);
float frgb = *reinterpret_cast<float*>(&rgb);
color.push_back(frgb);
}
if ((start + k) > maximas[i][j] && !pi && phases[i][k] > 0) pi = true;
if ((start + k) > maximas[i][j] && phases[i][k] < 0 && phases[i][k + 1] > 0 && pi)break;
}
}
}
return coordinate;
}
template <class ForwardIterator>
size_t CoreAlgorithm::argmin(ForwardIterator first, ForwardIterator last)
{
return std::distance(first, std::min_element(first, last));
}
template <class ForwardIterator>
size_t CoreAlgorithm::argmax(ForwardIterator first, ForwardIterator last)
{
return std::distance(first, std::max_element(first, last));
}
template <typename Tp>
vector<Tp> CoreAlgorithm::convertMat2Vector(const cv::Mat& mat)
{
return (vector<Tp>)(mat.reshape(1, 1)); //通道数不变，按行转为一行
}
void CoreAlgorithm::run()
{
tmp = OSTU(image);
auto min = false;
for (auto i = 0; i < 1024; i++)
Mat mask = Mat::zeros(Size(cols, rows), CV_32FC1);
for (auto i = 0; i < rows; i++)
{
for (auto j = 0; j < 1280; j++)
for (auto j = 0; j < cols; j++)
{
if (tmp.at<Vec3b>(i, j)[0] == 255)
mask.at<float>(i, j) = (int)rgbChannel[0].at<uchar>(i, j) > (int)rgbChannel[1].at<uchar>(i, j)
? (
(int)rgbChannel[0].at<uchar>(i, j) > (int)rgbChannel[2].at<uchar>(i, j)
? (int)rgbChannel[0].at<uchar>(i, j)
: (int)rgbChannel[2].at<uchar>(i, j))
: (
(int)rgbChannel[1].at<uchar>(i, j) > (int)rgbChannel[2].at<uchar>(i, j)
? (int)rgbChannel[1].at<uchar>(i, j)
: (int)rgbChannel[2].at<uchar>(i, j));
}
}
tmp = OtsuAlgThreshold(mask);
auto kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
morphologyEx(tmp, tmp, MORPH_OPEN, kernel);
auto min = false;
for (auto i = 0; i < rows; i++)
{
for (auto j = 0; j < cols; j++)
{
if (tmp.at<float>(i, j) == 255)
{
if (!min)
{
minX = i;
minY = j;
min = true;
}
@@ -229,117 +306,207 @@ void CoreAlgorithm::run()
}
}
minX -= 50;
minY -= 50;
maxX += 50;
maxY += 50;
Mat grayImage = Mat::zeros(cv::Size(1024, 1280), CV_32FC1);
for (auto i = 0; i < 1024; i++)
{
for (auto j = 0; j < 1280; j++)
{
grayImage.at<uchar>(i, j) = 0.2989 * channel.at(2).at<uchar>(i, j) +
0.5907 * channel.at(1).at<uchar>(i, j) +
0.1140 * channel.at(0).at<uchar>(i, j);
}
}
// cout << minX << "," << minY;
// cout << maxX << "," << maxY;
// cout<<rows<<","<<cols<<endl;
Mat featurePoint = Mat::zeros(cv::Size(1024, 1280), CV_32FC1);
// auto rec = image(Rect(minY, minX, maxY - minY, maxX - minX));
Mat img = Mat::zeros(Size(cols, rows), CV_32FC1);
for (auto i = minX; i < maxX; i++)
{
for (auto j = minY; j < maxY; j++)
{
auto temp = grayImage(Rect(i, j - neighborhood, 2 * neighborhood + 1, 1));
auto v = convertMat2Vector<uchar>(temp);
const auto tmpPosition1 = argmax(v.begin(), v.end());
const auto tmpPosition2 = argmin(v.begin(), v.end());
const auto position1 = static_cast<int>(tmpPosition1);
const auto position2 = static_cast<int>(tmpPosition2);
if (position1 == neighborhood)
{
featurePoint.at<uchar>(i, j) = 1;
}
if (position2 == neighborhood)
{
featurePoint.at<uchar>(i, j) = -1;
}
img.at<float>(i, j) = 0.2989 * (int)rgbChannel.at(2).at<uchar>(i, j) +
0.5907 * (int)rgbChannel.at(1).at<uchar>(i, j) +
0.1140 * (int)rgbChannel.at(0).at<uchar>(i, j);
}
}
kernel = getStructuringElement(MORPH_RECT, cv::Size(3, 3));
morphologyEx(img, img, MORPH_CLOSE, kernel);
GaussianBlur(img, img, Size(5, 5), 0, 0);
Mat derivative1 = Mat::zeros(Size(cols, rows), CV_32FC1);
Mat derivative2 = Mat::zeros(Size(cols, rows), CV_32FC1);
for (auto i = 0; i < rows; i++)
{
for (auto j = 1; j < cols - 1; j++)
{
derivative1.at<float>(i, j) = img.at<float>(i, j + 1) - img.at<float>(i, j);
derivative2.at<float>(i, j) = img.at<float>(i, j + 1) + img.at<float>(i, j - 1) - 2 * img.at<float>(i, j);
}
}
vector<vector<float>> maximas(0, vector<float>(0, 0));
vector<vector<float>> minimas(0, vector<float>(0, 0));
vector<vector<float>> colorLabel(0, vector<float>(0, 0));
for (auto i = minX; i < maxX; i++)
{
Mat temp = Mat::zeros(cv::Size(100, 1), CV_32FC1);
auto num = 0;
maximas.resize(i - minX + 1);
minimas.resize(i - minX + 1);
colorLabel.resize(i - minX + 1);
vector<double> tmpMin;
for (auto j = minY; j < maxY; j++)
{
if (featurePoint.at<uchar>(i, j) == 1)
// cout << i << endl;
if (derivative1.at<float>(i, j) > 0 && derivative1.at<float>(i, j + 1) < 0)
{
temp.at<uchar>(num, 0) = j;
int rh = channel.at(2).at<uchar>(i, j);
int gh = channel.at(1).at<uchar>(i, j);
int bh = channel.at(0).at<uchar>(i, j);
if (rh == gh || gh == bh || rh == bh)
double k = derivative1.at<float>(i, j + 1) - derivative1.at<float>(i, j);
double b = derivative1.at<float>(i, j) - k * j;
double zero = -b / k;
double k2 = derivative2.at<float>(i, j + 1) - derivative2.at<float>(i, j);
double b2 = derivative2.at<float>(i, j) - k2 * j;
double value = k2 * zero + b2;
if (value < 0 && lab.at<Vec3b>(i, zero)[0] > 5)
{
maximas[i - minX].push_back(zero);
if (lab.at<Vec3b>(i, zero)[2] < 126)
{
colorLabel[i - minX].push_back(2); //blue
}
else
{
if (lab.at<Vec3b>(i, zero)[1] >= 128)
{
colorLabel[i - minX].push_back(0); //red
}
else
{
colorLabel[i - minX].push_back(1); //green
}
}
}
}
if (derivative1.at<float>(i, j) < 0 && derivative1.at<float>(i, j + 1) > 0)
{
double k = derivative1.at<float>(i, j + 1) - derivative1.at<float>(i, j);
double b = derivative1.at<float>(i, j) - k * j;
double zero = -b / k;
double k2 = derivative2.at<float>(i, j + 1) - derivative2.at<float>(i, j);
double b2 = derivative2.at<float>(i, j) - k2 * j;
double value = k2 * zero + b2;
if (value > 0)
{
tmpMin.push_back(zero);
}
}
}
if (!tmpMin.empty() && !maximas[i - minX].empty())
{
auto pos = 0;
for (auto j = 0; j < tmpMin.size()-1; j++)
{
if (tmpMin[j + 1] < maximas[i - minX][pos])
{
featurePoint.at<uchar>(i, j) = -2;
continue;
}
auto h = Hsv(rh, gh, bh);
auto result = max_element(begin(h), end(h));
auto nPos = static_cast<int>(max_element(h.begin(), h.end()) - (h.begin()));
if (nPos == 0)
{
featurePoint.at<uchar>(i, j) = 0;
}
else if (nPos == 1)
{
featurePoint.at<uchar>(i, j) = 1;
}
else if (nPos == 2)
{
featurePoint.at<uchar>(i, j) = 2;
}
num = num + 1;
}
else if (featurePoint.at<uchar>(i, j) != -1)
{
featurePoint.at<uchar>(i, j) = -2;
minimas[i - minX].push_back(tmpMin[j]);
pos++;
if (pos >= maximas[i - minX].size())break;
}
}
}
auto number = 0;
vector<vector<float>> phases(0, vector<float>(0, 0));
emxArray_real_T* phase;
double x_data[1280] = {0};
int x_size[2] = {0};
emxInitArray_real_T(&phase, 2);
x_size[0] = 1;
for (auto i = minX; i < maxX; i++)
{
for (auto j = minY; j < maxY; j++)
phases.resize(i - minX + 1);
if (minimas[i - minX].empty())continue;
int start = minimas[i - minX][0];
int end = minimas[i - minX][minimas[i - minX].size() - 1];
x_size[1] = end - start;
for (auto j = start; j < end; j++)
{
if (featurePoint.at<uchar>(i, j) != -2) number++;
x_data[j - start] = (float)lab.at<Vec3b>(i, j)[0];
// if (i -minX== 300)
// cout<<x_data[j - start]<<",";
}
cwt(x_data, x_size, phase);
for (auto j = 0; j < x_size[1]; j++)
{
phases[i - minX].push_back(*(phase->data + j) / PI * 7);
// if (i - minX == 300)
// cout << j + start << "," << *(phase->data + j) << endl;
}
// if (i == 300) {
// for (auto j = 0; j < maximas[i - minX].size(); j++) {
// cout <<j+start<<","<< *(phase->data + int(maximas[i - minX][j] - start)) << endl;
//
// }
// }
}
auto db = DeBruijn(3, 4);
double map[76]{0};
for (auto i = 0; i < 61; i++)
{
int index = int(pow(3, 3) * db.at(i) + pow(3, 2) * db.at(i + 1) + 3 * db.at(i + 2) + db.at(i + 3));
map[index] = 7.5 + 14 * i;
}
Reconstruction(maximas, minimas, colorLabel, phases, cArg->getHc(), cArg->getHp(), map);
ofstream destFile("./Data/result/result.txt", ios::out); //以文本模式打开out.txt备写
for (auto i = 0; i < coordinate.size(); i++)
{
if (i == coordinate.size() - 1)
{
destFile << coordinate[i].at<float>(0, 0) << " " << coordinate[i].at<float>(0, 1) << " "
<< coordinate[i].at<float>(0, 2);
}
else
{
destFile << coordinate[i].at<float>(0, 0) << " " << coordinate[i].at<float>(0, 1) << " "
<< coordinate[i].at<float>(0, 2) << endl; //可以像用cout那样用ofstream对象
}
}
auto db = Debruijn(3, 4);
int p, q, t, u;
p = q = t = u = 3;
int sizes[] = {p, q, t, u};
auto all = p * q * t * u;
auto d1 = new float[all];
for (auto i = 0; i < all; i++)
{
d1[i] = i * 1;
}
auto map = Mat(4, sizes, CV_32S, d1);
for (auto i = 0; i < 61; i++)
{
auto id = map.step[0] * db[i] + map.step[1] * db[i + 1]
+ map.step[2] * db[i + 2] + map.step[3] * db[i + 3];
auto* stub = reinterpret_cast<int*>(map.data + id);
*stub = 7.5 + 14 * i;
}
auto coordinate = Reconstruction(featurePoint, number, cArg->getHc(), cArg->getHp(), map);
destFile.close();
saveCoordinate();
}
void CoreAlgorithm::saveCoordinate()
{
ofstream destFile("./Data/result/result.pcd", ios::out); //以文本模式打开out.txt备写
destFile << "# .PCD v0.7 - Point Cloud Data file format" << endl;
destFile << "VERSION 0.7" << endl;
destFile << "FIELDS x y z rgb" << endl;
destFile << "SIZE 4 4 4 4" << endl;
destFile << "TYPE F F F F" << endl;
destFile << "COUNT 1 1 1 1" << endl;
destFile << "WIDTH " << coordinate.size() << endl;
destFile << "HEIGHT 1" << endl;
destFile << "VIEWPOINT 0 0 0 1 0 0 0" << endl;
destFile << "POINTS " << coordinate.size() << endl;
destFile << "DATA ascii" << endl;
for (auto i = 0; i < coordinate.size(); i++)
{
// cout << i << endl;
if (i == coordinate.size() - 1)
{
destFile << coordinate[i].at<float>(0, 0) << " " << coordinate[i].at<float>(0, 1) << " "
<< coordinate[i].at<float>(0, 2) << " " << color[i];
}
else
{
destFile << coordinate[i].at<float>(0, 0) << " " << coordinate[i].at<float>(0, 1) << " "
<< coordinate[i].at<float>(0, 2) << " " << color[i] << endl; //可以像用cout那样用ofstream对象
}
}
destFile.close();
}

61
Classes/CoreAlgorithm.h
View File

@@ -1,7 +1,24 @@
#pragma once
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <string>
#include <vector>
#include <iostream>
#include <fstream>
#include <cmath>
#include <string>
#include <unordered_map>
#include <numeric>
#include "cwt.h"
#include "cwt_emxAPI.h"
#include "cwt_terminate.h"
#include "rt_nonfinite.h"
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
#include "CameraArguments.h"
using namespace std;
@@ -9,27 +26,31 @@ using namespace cv;
typedef unsigned char byte;
class CoreAlgorithm
{
# define PI acos(-1)
class CoreAlgorithm {
private:
Mat image, tmp;
vector<Mat> channel,hsv;
const int neighborhood = 5;
int minX = 0, maxX = 0, minY = 1280, maxY = 0;
CameraArguments* cArg;
Mat image, tmp, hsv, lab;
vector<Mat> rgbChannel, hsvChannel;
int minX = 0, maxX = 0, minY = 0, maxY = 0;
int rows, cols;
CameraArguments *cArg;
vector<Mat> coordinate;
vector<float> color;
private:
CoreAlgorithm(std::string path);
~CoreAlgorithm();
Mat OSTU(Mat src);
vector<int> Debruijn(int k, int n);
vector<int> Hsv(int r,int g,int b);
Mat Reconstruction(Mat featurePoint, int num,Mat Hc1,Mat Hp2,Mat map);
template <class ForwardIterator>
size_t argmin(ForwardIterator first, ForwardIterator last);
template <class ForwardIterator>
size_t argmax(ForwardIterator first, ForwardIterator last);
template <typename Tp>
std::vector<Tp> convertMat2Vector(const cv::Mat& mat);
static Mat OtsuAlgThreshold(Mat &src);
static vector<int> DeBruijn(int k, int n);
void Reconstruction(vector<vector<float>> maximas, vector<vector<float>> minimas, vector<vector<float>> colorLabel,
vector<vector<float>> phases,const Mat &Hc1, Mat Hp2, const double *map);
public:
void run();
CoreAlgorithm(const std::string &path, CameraArguments *cArgs);
~CoreAlgorithm();
void run();
void saveCoordinate();
};

47
Classes/Device.cpp Normal file
View File

@@ -0,0 +1,47 @@
#include "Device.h"
#include <QDebug>
Device* Device::instance = nullptr;
Device::Device()
{
vector<CameraInfo> cams = CameraPointGrey::getCameraList();
if (cams.size() != 0)
{
hasCamera = true;
camera = Camera::NewCamera(0, cams[0].busID, triggerModeSoftware);
CameraSettings camSettings;
camSettings.shutter = 25;
camSettings.gain = 0.0;
camera->setCameraSettings(camSettings);
camera->startCapture();
}
projector = new ProjectorLC4500(0);
if (projector->getIsRunning())hasProjector = true;
}
Device* Device::getInstance()
{
if (instance == nullptr) instance = new Device();
return instance;
}
Camera* Device::getCamera()
{
return camera;
}
Projector* Device::getProjector()
{
return projector;
}
bool Device::getHasCamera()
{
return hasCamera;
}
bool Device::getHasProjector()
{
return hasProjector;
}

28
Classes/Device.h Normal file
View File

@@ -0,0 +1,28 @@
#pragma once
#include "Camera.h"
#include "CameraPointGrey.h"
#include "ProjectorLC4500.h"
class Device
{
public:
static Device* instance;
static Device* getInstance();
Camera* getCamera();
Projector* getProjector();
bool getHasCamera();
bool getHasProjector();
private:
Device();
Camera *camera;
Projector *projector;
// variable indicate whether the camera and projector
// have been initialized or not
bool hasCamera = false;
bool hasProjector = false;
};

8
Classes/Help.cpp
View File

@@ -4,10 +4,10 @@ Help::Help(QWidget *parent)
: QWidget(parent)
{
ui.setupUi(this);
// QTextCodec *codec = QTextCodec::codecForName("utf-8"); //不起作用
// QTextCodec::setCodecForLocale(codec);
ui.textBrowser->setText(QString::fromUtf8("help")); // 暂时不能写显示中文
QPixmap *pixmap = new QPixmap(":/Reconstruction/image/reconstruction/help.png");
pixmap->scaled(ui.label->size(), Qt::KeepAspectRatio);
ui.label->setScaledContents(true);
ui.label->setPixmap(*pixmap);
}
Help::~Help()

4
Classes/Loading.cpp
View File

@@ -5,7 +5,7 @@ Loading::Loading(QWidget *parent)
{
setStyle();
setWindowFlags(Qt::FramelessWindowHint);
ui.label_2->hide();
// ui.progressBar->setValue(0);
// QGraphicsOpacityEffect *opacityEffect = new QGraphicsOpacityEffect;
@@ -14,7 +14,7 @@ Loading::Loading(QWidget *parent)
// todo ¼ÓÔØ
// ui.progressBar->setValue(100);
device = Device::getInstance();
ui.label->hide();
ui.label_2->show();
ready2Enter = true;

2
Classes/Loading.h
View File

@@ -5,6 +5,7 @@
#include "Reconstruction.h"
#include <QMouseEvent>
#include <QGraphicsOpacityEffect>
#include "Device.h"
class Loading : public QWidget
{
@@ -17,6 +18,7 @@ public:
private:
Ui::Loading ui;
bool ready2Enter = false;
Device* device;
int currentValue = 0;
void updateSlot();
void setStyle();

2
Classes/MyThread.cpp
View File

@@ -13,6 +13,8 @@ void MyThread::run()
io::loadPCDFile(pcd, *cloudPtr);
cloud = *cloudPtr;
}
}
void MyThread::setPcd(QString str)

1
Classes/MyThread.h
View File

@@ -3,6 +3,7 @@
#include <qthread.h>
#include <pcl/point_types.h>
#include <pcl/io/vtk_lib_io.h>
using namespace std;
using namespace pcl;

25
Classes/Projector.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef PROJECTOR_H
#define PROJECTOR_H
#include <iostream>
#include <vector>
// Abstract Projector base class
class Projector {
public:
// Interface function
Projector(){}
// Define preset pattern sequence
virtual void setPattern(unsigned int patternNumber , const unsigned char *tex , unsigned int texWidth , unsigned int texHeight) = 0;
virtual void displayPattern(unsigned int patternNumber) = 0;
// Upload and display pattern on the fly
virtual void displayTexture(const unsigned char *tex , unsigned int width , unsigned int height) = 0;
// Monochrome color display
virtual void displayBlack() = 0;
virtual void displayWhite() = 0;
virtual void getScreenRes(unsigned int *nx , unsigned int *ny) = 0;
virtual bool getIsRunning() = 0;
virtual ~Projector(){}
};
#endif

116
Classes/ProjectorLC4500.cpp Normal file
View File

@@ -0,0 +1,116 @@
#include "ProjectorLC4500.h"
#include <iostream>
#include "API.h"
#include "usb.h"
void showError(std::string err)
{
std::cerr << "lc4500startup: " << err.c_str() << std::endl;
}
ProjectorLC4500::ProjectorLC4500(unsigned int): nPatterns(0), isRunning(false)
{
std::cout << "ProjectorLC4500: preparing LightCrafter 4500 for duty... " << std::endl;
auto mark1 = false;
auto mark2 = false;
auto mark3 = false;
auto mark4 = false;
// Initialize usb connection
if (USB_Init())
{
showError("Could not init USB!");
}
else
{
mark1 = true;
}
if (USB_Open())
{
showError("Could not connect!");
}
else
{
mark2 = true;
}
if (!USB_IsConnected())
{
showError("Could not connect.");
}
else
{
mark3 = true;
}
// Make sure LC is not in standby
const bool standby = false;
if (!LCR_SetPowerMode(standby))
{
showError("Error Setting Power Mode");
}
else
{
mark4 = true;
}
if (mark1 && mark2 && mark3 && mark4) isRunning = true;
LCR_SetMode(false);
LCR_SetInputSource(2, 0);
LCR_SetPixelFormat(0);
}
void ProjectorLC4500::setPattern(unsigned int patternNumber, const unsigned char* tex, unsigned int texWidth,
unsigned int texHeight)
{
}
void ProjectorLC4500::displayPattern(unsigned int PatNum)
{
LCR_LoadSplash(PatNum);
rectangle croppedArea, displayArea;
croppedArea.firstPixel = 0;
croppedArea.firstLine = 0;
croppedArea.pixelsPerLine = 912;
croppedArea.linesPerFrame = 1140;
displayArea.firstPixel = 0;
displayArea.firstLine = 0;
displayArea.pixelsPerLine = 1280;
displayArea.linesPerFrame = 800;
LCR_SetDisplay(croppedArea, displayArea);
}
void ProjectorLC4500::displayTexture(const unsigned char* tex, unsigned int texWidth, unsigned int texHeight)
{
}
void ProjectorLC4500::displayBlack()
{
}
void ProjectorLC4500::displayWhite()
{
}
void ProjectorLC4500::getScreenRes(unsigned int* nx, unsigned int* ny)
{
*nx = 912;
*ny = 1140;
}
bool ProjectorLC4500::getIsRunning()
{
return isRunning;
}
ProjectorLC4500::~ProjectorLC4500()
{
// Stop pattern sequence
LCR_PatternDisplay(0);
USB_Close();
USB_Exit();
}

30
Classes/ProjectorLC4500.h Normal file
View File

@@ -0,0 +1,30 @@
#ifndef PROJECTORLC4500_H
#define PROJECTORLC4500_H
#include <iostream>
#include <vector>
#include <sys/types.h>
#include "Projector.h"
// Projecotr implementation for LightCrafter 4500 USB Api
class ProjectorLC4500 : public Projector {
public:
// Interface function
ProjectorLC4500(unsigned int);
// Define preset pattern sequence and upload to GPU
void setPattern(unsigned int patternNumber ,const unsigned char *tex ,unsigned int texWidth , unsigned int texHeight);
void displayPattern(unsigned int patternNumber);
// Upload and display pattern on the fly
void displayTexture(const unsigned char *tex , unsigned int width , unsigned int height);
void displayBlack();
void displayWhite();
void getScreenRes(unsigned int *nx , unsigned int *ny);
bool getIsRunning();
~ProjectorLC4500();
private:
unsigned int nPatterns;
bool isRunning;
};
#endif

430
Classes/Reconstruction.cpp
View File

@@ -1,12 +1,15 @@
#include "Reconstruction.h"
Reconstruction::Reconstruction(QWidget *parent)
Reconstruction::Reconstruction(QWidget* parent)
: QMainWindow(parent)
{
ui.setupUi(this);
calibData = new CalibrationData();
calibrator = new Calibrator();
dirModel = new TreeModel(this);
t = new MyThread;
connect(t, SIGNAL(finished()), this, SLOT(setCloud()));
device = Device::getInstance();
setStyle();
}
@@ -27,11 +30,11 @@ void Reconstruction::setStyle()
file.close();
QPalette palette1;
palette1.setColor(QPalette::Background, qRgba(62, 71, 128, 100)); //左侧
palette1.setColor(QPalette::Background, qRgba(62, 71, 128, 100)); //左侧
ui.widget->setPalette(palette1);
QPalette palette2;
palette2.setColor(QPalette::Background, qRgba(255, 255, 255, 100)); //白色
palette2.setColor(QPalette::Background, qRgba(255, 255, 255, 100)); //白色
// palette2.setColor(QPalette::Background, qRgba(204, 213, 240, 100)); //背景
ui.stackedWidget->setPalette(palette2);
@@ -39,12 +42,32 @@ void Reconstruction::setStyle()
setPicStyle();
setButtonStyle();
}
void Reconstruction::setPicStyle()
{
ui.label_9->setPixmap(QPixmap(":/icon/image/reconstruction/loading.png"));
ui.label_11->setPixmap(QPixmap(":/icon/image/calibration/novideo.png"));
if (device->getHasCamera())
{
auto w = ui.label_11->width();
auto h = ui.label_11->height();
Mat blackImg(h, w, CV_8UC1, cv::Scalar::all(0));
QImage qimage = cvtools::cvMat2qImage(blackImg);
QPixmap pixmap = QPixmap::fromImage(qimage);
ui.label_11->setPixmap(pixmap.scaled(w, h, Qt::KeepAspectRatio));
liveViewTimer = startTimer(100);
}
else
{
ui.label_11->setPixmap(QPixmap(":/icon/image/calibration/novideo.png"));
ui.pushButton_5->setEnabled(false);
ui.pushButton_6->setEnabled(false);
ui.pushButton_7->setEnabled(false);
ui.pushButton_8->setEnabled(false);
}
ui.label_21->setPixmap(QPixmap(":/icon/image/projection/novideo.jpg"));
}
void Reconstruction::setButtonStyle()
{
QString buttonStyle = "QPushButton{background-color:white;color: black;}"
@@ -73,12 +96,17 @@ void Reconstruction::setButtonStyle()
ui.pushButton_15->setIcon(QIcon(":/icon/image/reconstruction/save2.png"));
ui.pushButton_16->setIcon(QIcon(":/icon/image/reconstruction/color.png"));
}
#pragma endregion
#pragma endregion
#pragma region 界面菜单
void Reconstruction::on_pushButton_clicked()
{
ui.stackedWidget->setCurrentIndex(0);
// imgCount = 0;
// dirModel = new TreeModel(this);
// dirModel->clear();
// ui.treeView->setModel(dirModel);
// ui.stackedWidget->setCurrentIndex(0);
}
void Reconstruction::on_pushButton_2_clicked()
@@ -89,16 +117,17 @@ void Reconstruction::on_pushButton_2_clicked()
void Reconstruction::on_pushButton_3_clicked()
{
ui.stackedWidget->setCurrentIndex(2);
updateQVTK(cloud);
if(loadingStatus)
updateQVTK(cloud, color);
if (loadingStatus)
{
ui.label_9->setVisible(true);
}else
}
else
{
ui.label_9->setVisible(false);
}
}
#pragma endregion
#pragma endregion
#pragma region 多线程
void Reconstruction::setCloud()
@@ -106,9 +135,10 @@ void Reconstruction::setCloud()
ui.label_9->setVisible(false);
loadingStatus = false;
cloud = t->getCloud();
updateQVTK(cloud);
updateQVTK(cloud, color);
}
void Reconstruction::updateQVTK(PointCloud<PointXYZRGB> cloud)
void Reconstruction::updateQVTK(PointCloud<PointXYZRGB> cloud, QColor color)
{
boost::shared_ptr<visualization::PCLVisualizer> viewer(new visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0.458, 0.529, 0.844);
@@ -117,13 +147,18 @@ void Reconstruction::updateQVTK(PointCloud<PointXYZRGB> cloud)
{
PointCloud<PointXYZRGB>::Ptr cloudPtr(new PointCloud<PointXYZRGB>);
cloudPtr = cloud.makeShared();
viewer->addPointCloud(cloudPtr, "cloud");
int x = int(color.redF() * 255);
int y = int(color.greenF() * 255);
int z = int(color.blueF() * 255);
visualization::PointCloudColorHandlerCustom<PointXYZRGB> cloud_color(cloudPtr, x, y, z);// 统一处理点云颜色
viewer->addPointCloud(cloudPtr, cloud_color, "cloud");
viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
}
ui.qvtkWidget->SetRenderWindow(viewer->getRenderWindow());
ui.qvtkWidget->update();
}
#pragma endregion
#pragma endregion
#pragma region 系统标定-按钮
@@ -132,14 +167,15 @@ void Reconstruction::on_pushButton_5_clicked()
{
QString fileName = QFileDialog::getOpenFileName(
this, tr("open multiple image file"),
"./", tr("Image files(*.bmp *.jpg *.pbm *.pgm *.png *.ppm *.xbm *.xpm);;All files (*.*)")); // todo 文件类型待确认
"./", tr("Image files(*.bmp *.jpg *.pbm *.pgm *.png *.ppm *.xbm *.xpm);;All files (*.*)")); // todo 文件类型待确认
if (fileName.isEmpty())
{
QMessageBox mesg;
mesg.warning(this, "WARNING", "Failed to open picture");
return;
}else
}
else
{
calPath = fileName;
}
@@ -148,26 +184,96 @@ void Reconstruction::on_pushButton_5_clicked()
// 相机拍摄
void Reconstruction::on_pushButton_6_clicked()
{
if(calPath.isEmpty())
killTimer(liveViewTimer);
ui.pushButton_5->setEnabled(false);
ui.pushButton_6->setEnabled(false);
ui.pushButton_7->setEnabled(false);
ui.pushButton_8->setEnabled(false);
char path[100];
sprintf(path, "calib%.2d", imgCount);
fstools::mkDir(tr("./config/calib"), tr(path));
device->getProjector()->displayPattern(0);
//QTest::qSleep(500);
for (unsigned int i = 0; i < 44; i++)
{
QMessageBox mesg;
mesg.warning(this, "WARNING", "Haven't uploaded calibration pictures!");
}else
{
// todo 相机拍摄，存储照片集
// Project pattern
device->getProjector()->displayPattern(i);
//QTest::qSleep(200);
// Effectuate sleep (necessary with some camera implementations)
QApplication::processEvents();
// Aquire frame
auto frame = device->getCamera()->getFrame();
auto frameCV = cvtools::camFrame2Mat(frame);
cvtColor(frameCV, frameCV, cv::COLOR_BGR2GRAY);
//parameter
std::cout << cv::format("./config/calib/calib%.2d/%.2d.bmp", imgCount, i) << std::endl;
imwrite(cv::format("./config/calib/calib%.2d/%.2d.bmp", imgCount, i), frameCV);
auto qimage = cvtools::cvMat2qImage(frameCV);
auto w = ui.label_11->width();
auto h = ui.label_11->height();
auto pixmap = QPixmap::fromImage(qimage);
ui.label_11->setPixmap(pixmap.scaled(w, h, Qt::KeepAspectRatio));
}
imgCount++;
liveViewTimer = startTimer(100);
//parameter
setDirModel(tr("./config/calib"));
ui.treeView->setModel(dirModel);
ui.pushButton_5->setEnabled(true);
ui.pushButton_6->setEnabled(true);
ui.pushButton_7->setEnabled(true);
ui.pushButton_8->setEnabled(true);
// if (calPath.isEmpty())
// {
// QMessageBox mesg;
// mesg.warning(this, "WARNING", "Haven't uploaded calibration pictures!");
// }
// else
// {
// // todo 相机拍摄，存储照片集
// }
// 标定日志：显示拍摄照片集存储路径
ui.textBrowser_7->append("");
// ui.textBrowser_7->append("");
}
// 相机标定
void Reconstruction::on_pushButton_7_clicked()
{
int size = ui.spinBox->value();
int row = ui.spinBox_2->value();
int col = ui.spinBox_3->value();
// todo 相机标定
auto size = ui.spinBox->value();
auto row = ui.spinBox_2->value();
auto col = ui.spinBox_3->value();
calibrator->setCornerSize(size);
calibrator->setBoardRows(row);
calibrator->setBoardCols(col);
// todo 相机标定
killTimer(liveViewTimer);
calibrator->reset();
std::vector<Mat> imgList;
for (auto i = 0; i < dirModel->rowCount(); i++)
{
auto parent = dirModel->index(i, 0);
auto imgNum = dirModel->rowCount(parent);
for (auto j = 0; j < imgNum; j++)
{
imgList.push_back(getImage(i, j, GrayImageRole));
}
calibrator->addFrameSequence(imgList);
imgList.clear();
}
calibData = calibrator->calibrate();
std::cout << "calibration success!!!" << std::endl;
calibData->print();
ui.pushButton_6->setEnabled(true);
if (device->getCamera()->isConnecting()) liveViewTimer = startTimer(100);
// 相机参数栏：显示标定结果
ui.textBrowser->append("");
@@ -180,13 +286,13 @@ void Reconstruction::on_pushButton_7_clicked()
void Reconstruction::on_pushButton_8_clicked()
{
QFileDialog fileDialog;
QString fileName = fileDialog.getSaveFileName(this, "Open File", "", "Text File(*.txt)"); // todo 更改文件类型
QString fileName = fileDialog.getSaveFileName(this, "Open File", "", "Text File(*.txt)"); // todo 更改文件类型
if (fileName.isEmpty())
{
return;
}
QFile file(fileName);
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) // todo 更改文件类型
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) // todo 更改文件类型
{
QMessageBox::warning(this, "error", "open file failure!");
return;
@@ -194,9 +300,170 @@ void Reconstruction::on_pushButton_8_clicked()
else
{
// todo 保存结果
QString selectedFilter;
QString fileName = QFileDialog::getSaveFileName(this, QString::fromLocal8Bit("保存标定文件"), QString(), "*.xml",
&selectedFilter);
calibData->saveXML(fileName);
}
}
#pragma endregion
void Reconstruction::closeEvent(QCloseEvent*)
{
killTimer(liveViewTimer);
delete calibData;
delete calibrator;
delete dirModel;
}
void Reconstruction::timerEvent(QTimerEvent* event)
{
if (event->timerId() != liveViewTimer)
{
std::cerr << "Something fishy..." << std::endl << std::flush;
return;
}
auto frame = device->getCamera()->getFrame();
auto frameCV = cvtools::camFrame2Mat(frame);
cvtColor(frameCV, frameCV, cv::COLOR_BGR2GRAY);
if (frameCV.rows == 0 || frameCV.cols == 0)
{
return;
}
frameCV = frameCV.clone();
auto qimage = cvtools::cvMat2qImage(frameCV);
// correct size only if label has no borders/frame!
auto w = ui.label_11->width();
auto h = ui.label_11->height();
auto pixmap = QPixmap::fromImage(qimage);
ui.label_11->setPixmap(pixmap.scaled(w, h, Qt::KeepAspectRatio));
}
void Reconstruction::setDirModel(const QString& dirname)
{
QDir root_dir(dirname);
//reset internal data
dirModel->clear();
auto dirlist = root_dir.entryList(QDir::Dirs | QDir::NoDotAndDotDot, QDir::Name);
foreach(const QString & item, dirlist)
{
QDir dir(root_dir.filePath(item));
QStringList filters;
filters << "*.jpg" << "*.bmp" << "*.png";
auto filelist = dir.entryList(filters, QDir::Files, QDir::Name);
auto path = dir.path();
//setup the model
auto filecount = filelist.count();
if (filecount < 1)
{
//no images skip
continue;
}
unsigned row = dirModel->rowCount();
if (!dirModel->insertRow(row))
{
std::cout << "Failed model insert " << item.toStdString() << "(" << row << ")" << std::endl;
continue;
}
// add the childrens
auto parent = dirModel->index(row, 0);
dirModel->setData(parent, item, Qt::DisplayRole);
for (auto i = 0; i < filecount; i++)
{
const auto& filename = filelist.at(i);
if (!dirModel->insertRow(i, parent))
{
std::cout << "Failed model insert " << filename.toStdString() << "(" << row << ")" << std::endl;
break;
}
auto index = dirModel->index(i, 0, parent);
dirModel->setData(index, QString("#%1 %2").arg(i, 2, 10, QLatin1Char('0')).arg(filename), Qt::DisplayRole);
//additional data
dirModel->setData(index, path + "/" + filename, ImageFilenameRole);
}
}
if (dirModel->rowCount() >= 3)
{
ui.pushButton_7->setEnabled(true);
}
else
{
ui.pushButton_7->setEnabled(false);
}
}
Mat Reconstruction::getImage(unsigned level, unsigned n, Role role)
{
if (role != GrayImageRole && role != ColorImageRole)
{
//invalid args
return Mat();
}
//try to load
if (dirModel->rowCount() < static_cast<int>(level))
{
//out of bounds
return cv::Mat();
}
auto parent = dirModel->index(level, 0);
if (dirModel->rowCount(parent) < static_cast<int>(n))
{
//out of bounds
return Mat();
}
auto index = dirModel->index(n, 0, parent);
if (!index.isValid())
{
//invalid index
return Mat();
}
auto filename = dirModel->data(index, ImageFilenameRole).toString();
// std::cout << "[" << (role==GrayImageRole ? "gray" : "color") << "] Filename: " << filename.toStdString() << std::endl;
//load image
auto rgb_image = imread(filename.toStdString());
if (rgb_image.rows > 0 && rgb_image.cols > 0)
{
//color
if (role == ColorImageRole)
{
return rgb_image;
}
//gray scale
if (role == GrayImageRole)
{
Mat gray_image;
cvtColor(rgb_image, gray_image, cv::COLOR_BGR2GRAY);
return gray_image;
}
}
return Mat();
}
#pragma endregion
#pragma region 三维重建-按钮
@@ -206,7 +473,7 @@ void Reconstruction::on_pushButton_4_clicked()
// 选择投影图案
QString fileName = QFileDialog::getOpenFileName(
this, tr("open image file"),
"./", tr("Image files(*.bmp *.jpg *.pbm *.pgm *.png *.ppm *.xbm *.xpm);;All files (*.*)"));
"./", tr("Image files(*.bmp *.jpg *.pbm *.pgm *.png *.ppm *.xbm *.xpm);;All files (*.*)"));
if (fileName.isEmpty())
{
@@ -224,8 +491,8 @@ void Reconstruction::on_pushButton_4_clicked()
void Reconstruction::on_pushButton_9_clicked()
{
// todo 相机拍照
picPath = "Resources/image/test.png"; // 该行仅做测试使用
DisplayPic *picDlg = new DisplayPic();
picPath = "Resources/image/test.png"; // 该行仅做测试使用
DisplayPic* picDlg = new DisplayPic();
picDlg->setPicPath(picPath);
connect(picDlg, SIGNAL(getPicAction(QString)), this, SLOT(setPicAction(QString)));
picDlg->show();
@@ -233,11 +500,12 @@ void Reconstruction::on_pushButton_9_clicked()
void Reconstruction::setPicAction(QString action)
{
if(action=="confirmed")
if (action == "confirmed")
{
qDebug("confirmed");
confirmPic = true;
}else if(action=="canceled")
}
else if (action == "canceled")
{
qDebug("canceled");
confirmPic = false;
@@ -248,28 +516,30 @@ void Reconstruction::setPicAction(QString action)
void Reconstruction::on_pushButton_10_clicked()
{
// todo 保存照片
if(confirmPic)
if (confirmPic)
{
QString fileName = QFileDialog::getSaveFileName(this,
tr("save image"),
"",
tr("*.png;; *.jpg;; *.bmp;; All files(*.*)"));
tr("save image"),
"",
tr("*.png;; *.jpg;; *.bmp;; All files(*.*)"));
if (!fileName.isNull())
{
QImage iim(picPath);//创建一个图片对象,存储源图片
QPainter painter(&iim);//设置绘画设备
QFile file(fileName);//创建一个文件对象，存储用户选择的文件
if (!file.open(QIODevice::ReadWrite)) {
return;
QImage iim(picPath); //创建一个图片对象,存储源图片
QPainter painter(&iim); //设置绘画设备
QFile file(fileName); //创建一个文件对象，存储用户选择的文件
if (!file.open(QIODevice::ReadWrite))
{
return;
}
QByteArray ba;
QBuffer buffer(&ba);
buffer.open(QIODevice::WriteOnly);
iim.save(&buffer, "JPG");//把图片以流方式写入文件缓存流中
file.write(ba);//将流中的图片写入文件对象当中
iim.save(&buffer, "JPG"); //把图片以流方式写入文件缓存流中
file.write(ba); //将流中的图片写入文件对象当中
}
}else
}
else
{
QMessageBox mesg;
mesg.warning(this, "WARNING", "Haven't taken picture!");
@@ -280,9 +550,52 @@ void Reconstruction::on_pushButton_10_clicked()
void Reconstruction::on_pushButton_17_clicked()
{
// todo 开始重建
// auto cArg = CameraArguments::getInstance();
Mat r(3, 3, CV_32F);
double m0[3][3] = {
{9.7004457782050868e-001, 1.3447278830863673e-002, 2.4255450466457243e-001},
{-8.7082927494022376e-003, 9.9974988338843274e-001, -2.0599424802792338e-002},
{-2.4277084396282392e-001, 1.7870124701864658e-002, 9.6991905639837694e-001}
};
for (auto i = 0; i < r.rows; i++)
for (auto j = 0; j < r.cols; j++)
r.at<float>(i, j) = m0[i][j];
Mat t(1, 3, CV_32F);
double m1[1][3] = {
{-1.9511179496234658e+002, 1.2627509817628756e+001, -5.9345885017522171e+001}
};
for (auto i = 0; i < t.rows; i++)
for (auto j = 0; j < t.cols; j++)
t.at<float>(i, j) = m1[i][j];
Mat kc(3, 3, CV_32F);
double m2[3][3] = {
{2.1536653255083029e+003, 0., 6.1886776197116581e+002},
{0., 2.1484363899666910e+003, 5.0694898820460787e+002},
{0., 0., 1.}
};
for (auto i = 0; i < kc.rows; i++)
for (auto j = 0; j < kc.cols; j++)
kc.at<float>(i, j) = m2[i][j];
Mat kp(3, 3, CV_32F);
double m3[3][3] = {
{1.7235093158297350e+003, 0., 4.4128195628736904e+002},
{0., 3.4533404000869359e+003, 5.7316457428558715e+002},
{0., 0., 1.}
};
for (auto i = 0; i < kp.rows; i++)
for (auto j = 0; j < kp.cols; j++)
kp.at<float>(i, j) = m3[i][j];
auto cArg = CameraArguments::getInstance(r, t, kc, kp);
CoreAlgorithm testCase = CoreAlgorithm("./Data/image/reconstruction/tq.png", cArg);
testCase.run();
}
#pragma endregion
#pragma endregion
#pragma region 点云渲染-按钮
@@ -301,7 +614,7 @@ void Reconstruction::on_pushButton_12_clicked()
// 导入点云
void Reconstruction::on_pushButton_13_clicked()
{
if(loadingStatus)
if (loadingStatus)
{
QMessageBox mesg;
mesg.warning(this, "WARNING", "正在加载… ");
@@ -309,8 +622,8 @@ void Reconstruction::on_pushButton_13_clicked()
}
QString fileName = QFileDialog::getOpenFileName(
this, tr("open multiple image file"),
"./", tr("PCD files(*.pcd);;All files (*.*)")); // todo 文件类型待确认
this, tr("open multiple image file"),
"./", tr("PCD files(*.pcd);;All files (*.*)")); // todo 文件类型待确认
if (fileName.isEmpty())
{
@@ -338,9 +651,9 @@ void Reconstruction::on_pushButton_14_clicked()
void Reconstruction::on_pushButton_15_clicked()
{
QString fileName = QFileDialog::getSaveFileName(this,
tr("save screen shot"),
"",
tr("*.png;; *.jpg;; *.bmp;; All files(*.*)"));
tr("save screen shot"),
"",
tr("*.png;; *.jpg;; *.bmp;; All files(*.*)"));
if (!fileName.isNull())
{
@@ -353,13 +666,14 @@ void Reconstruction::on_pushButton_15_clicked()
// 颜色选取
void Reconstruction::on_pushButton_16_clicked()
{
color = QColorDialog::getColor(Qt::black);
if (color.isValid()){
// qDebug("x:%f, %f, %f",color.redF(), color.greenF(), color.blueF());
// todo 颜色选取框已选择颜色color，接下来对color进行处理
QColor colortmp = QColorDialog::getColor(Qt::black);
if (colortmp.isValid()) {
color = colortmp;
updateQVTK(cloud, color);
}
}
// 帮助
void Reconstruction::on_pushButton_18_clicked()
{

27
Classes/Reconstruction.h
View File

@@ -7,6 +7,7 @@
#include <QBuffer>
#include <QTextStream>
#include <QStyleFactory>
#include <QtTest/QtTest>
#include "DisplayPic.h"
#include "ui_Reconstruction.h"
#include <pcl/point_types.h>
@@ -15,33 +16,55 @@
#include <pcl/io/vtk_lib_io.h>
#include <vtkRenderWindow.h>
#include <QProgressDialog>
#include "Camera.h"
#include "Projector.h"
#include "fstools.h"
#include "TreeModel.h"
#include "Calibrator.h"
#include "CalibrationData.h"
#include "Device.h"
#include "CoreAlgorithm.h"
#include "MyThread.h"
#include "Help.h"
#include <iostream>
using namespace pcl;
using namespace std;
enum Role { ImageFilenameRole = Qt::UserRole, GrayImageRole, ColorImageRole };
class Reconstruction : public QMainWindow
{
Q_OBJECT
public:
Reconstruction(QWidget *parent = Q_NULLPTR);
void timerEvent(QTimerEvent* event);
void closeEvent(QCloseEvent*);
void setDirModel(const QString& dirname);
Mat getImage(unsigned level, unsigned n, Role role);
private:
Ui::ReconstructionClass ui;
Device* device;
QString calPath; // 系统标定：标定图像的存储路径
QString picPath = "Result/result.png"; // 三维重建：拍摄照片的存储路径
PointCloud<PointXYZRGB> cloud;
bool confirmPic = false; // 三维重建：确定是否用所拍照片进行重建
QColor color = Qt::black; // 点云渲染：颜色
int liveViewTimer;
TreeModel* dirModel;
CalibrationData* calibData;
Calibrator* calibrator;
int imgCount;
// 多线程
MyThread* t;
bool loadingStatus = false; // 点云渲染
void setStyle();
void setPicStyle();
void setButtonStyle();
void updateQVTK(PointCloud<PointXYZRGB> cloud);
void updateQVTK(PointCloud<PointXYZRGB> cloud, QColor color);
private slots:
void on_pushButton_clicked();
@@ -64,4 +87,4 @@ private slots:
void on_pushButton_18_clicked();
void setPicAction(QString action);
void setCloud();
};
};

205
Classes/TreeModel.cpp Normal file
View File

@@ -0,0 +1,205 @@
#include "TreeModel.h"
TreeModel::TreeModel(QObject * parent) :
QAbstractItemModel(parent) ,
_columnCount(1) ,
_horizontalHeader(QList<QString>()<<QString::fromLocal8Bit("")) ,
_root()
{
}
TreeModel::TreeModel(unsigned columns , QObject * parent) :
QAbstractItemModel(parent) ,
_columnCount(columns) ,
_root()
{
}
TreeModel::Item * TreeModel::get_item(const QModelIndex & index)
{
return reinterpret_cast<Item *>(index.internalPointer());
}
const TreeModel::Item * TreeModel::get_item(const QModelIndex & index) const
{
return const_cast<TreeModel *>(this)->get_item(index);
}
QModelIndex TreeModel::index(int row , int column , const QModelIndex & parent) const
{
const Item * item = get_item(parent);
if (!item)
{
item = &_root;
}
return createIndex(row , column , const_cast<Item *>(item->child(row)));
}
QModelIndex TreeModel::parent(const QModelIndex & index) const
{
const Item * item = get_item(index);
if (item)
{
const Item * parent = item->parent();
if (parent)
{
const Item * grand_parent = parent->parent();
int row = 0;
if (grand_parent)
{
row = grand_parent->childRow(parent);
}
return createIndex(row , 0 , const_cast<Item *>(parent));
}
}
return QModelIndex();
}
int TreeModel::rowCount(const QModelIndex & parent) const
{
const Item * item = get_item(parent);
if (!item)
{
item = &_root;
}
return item->childrenCount();
}
int TreeModel::columnCount(const QModelIndex & parent) const
{
return _columnCount;
}
QVariant TreeModel::data(const QModelIndex & index , int role) const
{
const Item * item = get_item(index);
if (item)
{
return item->data(role);
}
//not found
return QVariant();
}
QVariant TreeModel::headerData(int section , Qt::Orientation orientation , int role) const
{
if (orientation==Qt::Horizontal && role==Qt::DisplayRole && section<_horizontalHeader.size())
{
return _horizontalHeader.at(section);
}
return QVariant();
}
bool TreeModel::setData(const QModelIndex & index , const QVariant & value, int role)
{
Item * item = get_item(index);
if (item)
{
item->setData(value , role);
return true;
}
//error
return false;
}
bool TreeModel::insertRow(int row , const QModelIndex & parent)
{
Item * item = get_item(parent);
if (!item)
{
item = &_root;
}
beginInsertRows(parent , row ,row);
bool rv = item->insertRow(row);
endInsertRows();
return rv;
}
void TreeModel::clear(void)
{
//removeRows(0 rowCount());
beginResetModel();
_root.clear();
endResetModel();
}
unsigned TreeModel::Item::next_id = 0;
TreeModel::Item::Item() : _id(next_id++) , _parent(NULL), _data(), _children()
{
}
void TreeModel::Item::clear(void)
{
_children.clear();
_data.clear();
}
bool TreeModel::Item::insertRow(int row)
{
if (row>=0 && row<=_children.size())
{
_children.insert(row , Item());
_children[row].setParent(this);
return true;
}
return false;
}
void TreeModel::Item::setData(const QVariant & value, int role)
{
_data.insert(role , value);
}
QVariant TreeModel::Item::data(int role) const
{
QMap<int , QVariant>::const_iterator iter = _data.constFind(role);
if (iter!=_data.constEnd())
{
return *iter;
}
return QVariant();
}
int TreeModel::Item::childrenCount(void) const
{
return _children.size();
}
TreeModel::Item * TreeModel::Item::child(int index)
{
if (index>=0 && index<_children.size())
{
return &(_children[index]);
}
return NULL;
}
const TreeModel::Item * TreeModel::Item::child(int index) const
{
return const_cast<TreeModel::Item *>(this)->child(index);
}
int TreeModel::Item::childRow(const TreeModel::Item * child) const
{
if (!child)
{
return -1;
}
int index = 0;
foreach(const TreeModel::Item & item, _children)
{
if (item.id()==child->id())
{
return index;
}
index++;
}
return -1;
}

68
Classes/TreeModel.h Normal file
View File

@@ -0,0 +1,68 @@
#ifndef __TREEMODEL_HPP__
#define __TREEMODEL_HPP__
#include <QtCore/QObject>
#include <QtCore/qabstractitemmodel.h>
class TreeModel : public QAbstractItemModel
{
class Item
{
public:
Item();
inline unsigned id(void) const {return _id;}
void clear(void);
bool insertRow(int row);
QVariant data(int role) const;
void setData(const QVariant & value, int role);
inline Item * parent(void) const {return _parent;}
inline void setParent(Item * parent) {_parent = parent;}
int childrenCount(void) const;
const Item * child(int index) const;
Item * child(int index);
int childRow(const Item * child) const;
private:
unsigned _id;
Item * _parent;
QMap<int, QVariant> _data;
QList<Item> _children;
static unsigned next_id;
};
public:
TreeModel(QObject * parent = 0);
TreeModel(unsigned columns, QObject * parent = 0);
/* Read-Only model members */
virtual QModelIndex index(int row , int column, const QModelIndex & parent = QModelIndex()) const;
virtual QModelIndex parent(const QModelIndex & index) const;
virtual int rowCount(const QModelIndex & parent = QModelIndex()) const;
virtual int columnCount(const QModelIndex & parent = QModelIndex()) const;
virtual QVariant data(const QModelIndex & index, int role) const;
virtual QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const;
/* Editable Model members */
bool insertRow(int row, const QModelIndex & parent = QModelIndex());
virtual bool setData(const QModelIndex & index, const QVariant & value, int role = Qt::EditRole);
void clear(void);
private:
Item * get_item(const QModelIndex & index);
const Item * get_item(const QModelIndex & index) const;
private:
unsigned _columnCount;
QList<QString> _horizontalHeader;
Item _root;
};
#endif //__TREEMODEL_HPP__

94
Classes/bsxfun.cpp Normal file
View File

@@ -0,0 +1,94 @@
//
// File: bsxfun.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "bsxfun.h"
#include "cwt.h"
#include "cwt_emxutil.h"
#include "rt_nonfinite.h"
#include <string.h>
// Function Definitions
//
// Arguments : const creal_T a_data[]
// const int a_size[2]
// const emxArray_real_T *b
// emxArray_creal_T *c
// Return Type : void
//
void bsxfun(const creal_T a_data[], const int a_size[2], const emxArray_real_T
*b, emxArray_creal_T *c)
{
int u0;
int u1;
int csz_idx_1;
int i;
int acoef;
int bcoef;
int b_bcoef;
int i1;
int k;
double d;
u0 = b->size[1];
u1 = a_size[1];
if (u0 < u1) {
u1 = u0;
}
if (b->size[1] == 1) {
csz_idx_1 = a_size[1];
} else if (a_size[1] == 1) {
csz_idx_1 = b->size[1];
} else if (a_size[1] == b->size[1]) {
csz_idx_1 = a_size[1];
} else {
csz_idx_1 = u1;
}
i = c->size[0] * c->size[1];
c->size[0] = b->size[0];
u0 = b->size[1];
u1 = a_size[1];
if (u0 < u1) {
u1 = u0;
}
if (b->size[1] == 1) {
c->size[1] = a_size[1];
} else if (a_size[1] == 1) {
c->size[1] = b->size[1];
} else if (a_size[1] == b->size[1]) {
c->size[1] = a_size[1];
} else {
c->size[1] = u1;
}
emxEnsureCapacity_creal_T(c, i);
if ((b->size[0] != 0) && (csz_idx_1 != 0)) {
acoef = (a_size[1] != 1);
bcoef = (b->size[1] != 1);
i = csz_idx_1 - 1;
for (u0 = 0; u0 <= i; u0++) {
u1 = acoef * u0;
csz_idx_1 = bcoef * u0;
b_bcoef = (b->size[0] != 1);
i1 = c->size[0] - 1;
for (k = 0; k <= i1; k++) {
d = b->data[b_bcoef * k + b->size[0] * csz_idx_1];
c->data[k + c->size[0] * u0].re = d * a_data[u1].re;
c->data[k + c->size[0] * u0].im = d * a_data[u1].im;
}
}
}
}
//
// File trailer for bsxfun.cpp
//
// [EOF]
//

27
Classes/bsxfun.h Normal file
View File

@@ -0,0 +1,27 @@
//
// File: bsxfun.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef BSXFUN_H
#define BSXFUN_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern void bsxfun(const creal_T a_data[], const int a_size[2], const
emxArray_real_T *b, emxArray_creal_T *c);
#endif
//
// File trailer for bsxfun.h
//
// [EOF]
//

66
Classes/cvtools.cpp Normal file
View File

@@ -0,0 +1,66 @@
#include "cvtools.h"
#include <vector>
#include "Camera.h"
namespace cvtools {
QImage cvMat2qImage(cv::Mat mat){
// 8-bits unsigned, NO. OF CHANNELS=1
if(mat.type()==CV_8UC1) {
// Set the color table (used to tranMVate colour indexes to qRgb values)
QVector<QRgb> colorTable;
for (int i=0; i<256; i++)
colorTable.push_back(qRgb(i,i,i));
// Copy input Mat
QImage img((const uchar*)mat.data, mat.cols, mat.rows, mat.step, QImage::Format_Indexed8);
img.setColorTable(colorTable);
return img;
} else if(mat.type()==CV_8UC3) {
// Copy input Mat
QImage img((const uchar*)mat.data, mat.cols, mat.rows, mat.step, QImage::Format_RGB888);
return img;
} else if(mat.type()==CV_16UC1) {
mat.convertTo(mat, CV_8UC1, 1.0/256.0);
return cvMat2qImage(mat);
} else if(mat.type()==CV_16UC3) {
mat.convertTo(mat, CV_8UC3, 1.0/256.0);
return cvMat2qImage(mat);
} else if(mat.type()==CV_32FC1) {
cv::Mat rgb(mat.size(), CV_32FC3);
rgb.addref();
cv::cvtColor(mat, rgb, cv::COLOR_GRAY2RGB);
// Copy input Mat
QImage img((const uchar*)rgb.data, rgb.cols, rgb.rows, rgb.step, QImage::Format_RGB32);
rgb.release();
return img;
} else {
std::cerr << "SMVideoWidget: cv::Mat could not be converted to QImage!";
return QImage();
}
}
cv::Mat Matx33dToMat(cv::Matx33d mat) {
return cv::Mat(mat);
}
cv::Mat Vec3fToMat(cv::Vec<float, 3> vec) {
return (cv::Mat(vec));
}
cv::Mat camFrame2Mat(CameraFrame camFrame) {
cv::Mat frameCV(camFrame.height, camFrame.width, CV_8UC3, camFrame.memory);
frameCV = frameCV.clone();
std::vector<cv::Mat> bgr, rgb;
split(frameCV, bgr);
rgb.resize(3);
rgb[0] = bgr[2];
rgb[1] = bgr[1];
rgb[2] = bgr[0];
cv::merge(rgb, frameCV);
return frameCV;
}
}

18
Classes/cvtools.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef CVTOOLS_H
#define CVTOOLS_H
#include <opencv2/opencv.hpp>
#include <QtGui/qimage.h>
#include "Camera.h"
// struct CameraFrame;
namespace cvtools
{
QImage cvMat2qImage(cv::Mat mat);
cv::Mat Matx33dToMat(cv::Matx33d mat);
cv::Mat Vec3fToMat(cv::Vec<float, 3> vec);
cv::Mat camFrame2Mat(CameraFrame camFrame);
}
#endif // CVTOOLS_H

291
Classes/cwt.cpp Normal file
View File

@@ -0,0 +1,291 @@
//
// File: cwt.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "cwt.h"
#include "cwt_data.h"
#include "cwt_emxutil.h"
#include "cwt_initialize.h"
#include "cwtfilterbank.h"
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include <cmath>
#include <cstring>
#include <math.h>
#include <string.h>
// Function Declarations
static double rt_atan2d_snf(double u0, double u1);
static double rt_hypotd_snf(double u0, double u1);
// Function Definitions
//
// Arguments : double u0
// double u1
// Return Type : double
//
static double rt_atan2d_snf(double u0, double u1)
{
double y;
int b_u0;
int b_u1;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else if (rtIsInf(u0) && rtIsInf(u1)) {
if (u0 > 0.0) {
b_u0 = 1;
} else {
b_u0 = -1;
}
if (u1 > 0.0) {
b_u1 = 1;
} else {
b_u1 = -1;
}
y = atan2(static_cast<double>(b_u0), static_cast<double>(b_u1));
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
//
// Arguments : double u0
// double u1
// Return Type : double
//
static double rt_hypotd_snf(double u0, double u1)
{
double y;
double a;
a = std::abs(u0);
y = std::abs(u1);
if (a < y) {
a /= y;
y *= std::sqrt(a * a + 1.0);
} else if (a > y) {
y /= a;
y = a * std::sqrt(y * y + 1.0);
} else {
if (!rtIsNaN(y)) {
y = a * 1.4142135623730951;
}
}
return y;
}
//
// Arguments : const double x_data[]
// const int x_size[2]
// emxArray_real_T *phase
// Return Type : void
//
void cwt(const double x_data[], const int x_size[2], emxArray_real_T *phase)
{
cwtfilterbank fb;
int b_x_size[2];
int loop_ub;
double b_x_data[1280];
emxArray_creal_T *cfs;
emxArray_creal_T *max_s;
int m;
int n;
int i;
boolean_T SCALEA;
double ma;
boolean_T SCALEB;
double x;
double absbi;
double absai;
double absbr;
double Ma;
if (isInitialized_cwt == false) {
cwt_initialize();
}
emxInitStruct_cwtfilterbank(&fb);
cwtfilterbank_cwtfilterbank(&fb, static_cast<double>(x_size[1]));
b_x_size[0] = 1;
b_x_size[1] = x_size[1];
loop_ub = x_size[0] * x_size[1] - 1;
if (0 <= loop_ub) {
std::memcpy(&b_x_data[0], &x_data[0], (loop_ub + 1) * sizeof(double));
}
emxInit_creal_T(&cfs, 2);
emxInit_creal_T(&max_s, 2);
cwtfilterbank_wt(&fb, b_x_data, b_x_size, cfs);
m = cfs->size[0];
n = cfs->size[1];
i = max_s->size[0] * max_s->size[1];
max_s->size[0] = 1;
max_s->size[1] = cfs->size[1];
emxEnsureCapacity_creal_T(max_s, i);
if (cfs->size[1] >= 1) {
for (loop_ub = 0; loop_ub < n; loop_ub++) {
max_s->data[loop_ub] = cfs->data[cfs->size[0] * loop_ub];
for (i = 2; i <= m; i++) {
if (rtIsNaN(cfs->data[(i + cfs->size[0] * loop_ub) - 1].re) || rtIsNaN
(cfs->data[(i + cfs->size[0] * loop_ub) - 1].im)) {
SCALEA = false;
} else if (rtIsNaN(max_s->data[loop_ub].re) || rtIsNaN(max_s->
data[loop_ub].im)) {
SCALEA = true;
} else {
ma = std::abs(max_s->data[loop_ub].re);
if ((ma > 8.9884656743115785E+307) || (std::abs(max_s->data[loop_ub].
im) > 8.9884656743115785E+307)) {
SCALEA = true;
} else {
SCALEA = false;
}
if ((std::abs(cfs->data[(i + cfs->size[0] * loop_ub) - 1].re) >
8.9884656743115785E+307) || (std::abs(cfs->data[(i + cfs->size[0]
* loop_ub) - 1].im) > 8.9884656743115785E+307)) {
SCALEB = true;
} else {
SCALEB = false;
}
if (SCALEA || SCALEB) {
x = rt_hypotd_snf(max_s->data[loop_ub].re / 2.0, max_s->data[loop_ub]
.im / 2.0);
absbi = rt_hypotd_snf(cfs->data[(i + cfs->size[0] * loop_ub) - 1].re
/ 2.0, cfs->data[(i + cfs->size[0] * loop_ub)
- 1].im / 2.0);
} else {
x = rt_hypotd_snf(max_s->data[loop_ub].re, max_s->data[loop_ub].im);
absbi = rt_hypotd_snf(cfs->data[(i + cfs->size[0] * loop_ub) - 1].re,
cfs->data[(i + cfs->size[0] * loop_ub) - 1].im);
}
if (x == absbi) {
absai = std::abs(max_s->data[loop_ub].im);
absbr = std::abs(cfs->data[(i + cfs->size[0] * loop_ub) - 1].re);
absbi = std::abs(cfs->data[(i + cfs->size[0] * loop_ub) - 1].im);
if (ma > absai) {
Ma = ma;
ma = absai;
} else {
Ma = absai;
}
if (absbr > absbi) {
absai = absbr;
absbr = absbi;
} else {
absai = absbi;
}
if (Ma > absai) {
if (ma < absbr) {
x = Ma - absai;
absbi = (ma / 2.0 + absbr / 2.0) / (Ma / 2.0 + absai / 2.0) *
(absbr - ma);
} else {
x = Ma;
absbi = absai;
}
} else if (Ma < absai) {
if (ma > absbr) {
absbi = absai - Ma;
x = (ma / 2.0 + absbr / 2.0) / (Ma / 2.0 + absai / 2.0) * (ma -
absbr);
} else {
x = Ma;
absbi = absai;
}
} else {
x = ma;
absbi = absbr;
}
if (x == absbi) {
x = rt_atan2d_snf(max_s->data[loop_ub].im, max_s->data[loop_ub].re);
absbi = rt_atan2d_snf(cfs->data[(i + cfs->size[0] * loop_ub) - 1].
im, cfs->data[(i + cfs->size[0] * loop_ub) -
1].re);
if (x == absbi) {
absbi = cfs->data[(i + cfs->size[0] * loop_ub) - 1].re;
absai = cfs->data[(i + cfs->size[0] * loop_ub) - 1].im;
if (x > 0.78539816339744828) {
if (x > 2.3561944901923448) {
x = -max_s->data[loop_ub].im;
absbi = -absai;
} else {
x = -max_s->data[loop_ub].re;
absbi = -absbi;
}
} else if (x > -0.78539816339744828) {
x = max_s->data[loop_ub].im;
absbi = absai;
} else if (x > -2.3561944901923448) {
x = max_s->data[loop_ub].re;
} else {
x = -max_s->data[loop_ub].im;
absbi = -absai;
}
if (x == absbi) {
x = 0.0;
absbi = 0.0;
}
}
}
}
SCALEA = (x < absbi);
}
if (SCALEA) {
max_s->data[loop_ub] = cfs->data[(i + cfs->size[0] * loop_ub) - 1];
}
}
}
}
emxFree_creal_T(&cfs);
i = phase->size[0] * phase->size[1];
phase->size[0] = 1;
phase->size[1] = max_s->size[1];
emxEnsureCapacity_real_T(phase, i);
loop_ub = max_s->size[1];
for (i = 0; i < loop_ub; i++) {
phase->data[i] = 0.0;
}
i = max_s->size[1];
for (loop_ub = 0; loop_ub < i; loop_ub++) {
phase->data[loop_ub] = std::atan(max_s->data[loop_ub].im / max_s->
data[loop_ub].re);
}
emxFree_creal_T(&max_s);
//
emxFreeStruct_cwtfilterbank(&fb);
}
//
// File trailer for cwt.cpp
//
// [EOF]
//

27
Classes/cwt.h Normal file
View File

@@ -0,0 +1,27 @@
//
// File: cwt.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef CWT_H
#define CWT_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern void cwt(const double x_data[], const int x_size[2], emxArray_real_T
*phase);
#endif
//
// File trailer for cwt.h
//
// [EOF]
//

33
Classes/cwt_data.cpp Normal file
View File

@@ -0,0 +1,33 @@
//
// File: cwt_data.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "cwt_data.h"
#include "cwt.h"
#include "rt_nonfinite.h"
#include <string.h>
// Variable Definitions
omp_nest_lock_t emlrtNestLockGlobal;
const char cv1[128] = { '\x00', '\x01', '\x02', '\x03', '\x04', '\x05', '\x06',
'\x07', '\x08', ' ', '\x0a', '\x0b', '\x0c', '\x0d', '\x0e', '\x0f', '\x10',
'\x11', '\x12', '\x13', '\x14', '\x15', '\x16', '\x17', '\x18', '\x19', '\x1a',
'\x1b', '\x1c', '\x1d', '\x1e', '\x1f', ' ', '!', '\"', '#', '$', '%', '&',
'\'', '(', ')', '*', '+', ',', '-', '.', '/', '0', '1', '2', '3', '4', '5',
'6', '7', '8', '9', ':', ';', '<', '=', '>', '?', '@', 'a', 'b', 'c', 'd', 'e',
'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u',
'v', 'w', 'x', 'y', 'z', '[', '\\', ']', '^', '_', '`', 'a', 'b', 'c', 'd',
'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't',
'u', 'v', 'w', 'x', 'y', 'z', '{', '|', '}', '~', '\x7f' };
boolean_T isInitialized_cwt = false;
//
// File trailer for cwt_data.cpp
//
// [EOF]
//

28
Classes/cwt_data.h Normal file
View File

@@ -0,0 +1,28 @@
//
// File: cwt_data.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef CWT_DATA_H
#define CWT_DATA_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Variable Declarations
extern omp_nest_lock_t emlrtNestLockGlobal;
extern const char cv1[128];
extern boolean_T isInitialized_cwt;
#endif
//
// File trailer for cwt_data.h
//
// [EOF]
//

130
Classes/cwt_emxAPI.cpp Normal file
View File

@@ -0,0 +1,130 @@
//
// File: cwt_emxAPI.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "cwt_emxAPI.h"
#include "cwt.h"
#include "cwt_emxutil.h"
#include "rt_nonfinite.h"
#include <cstdlib>
#include <string.h>
// Function Definitions
//
// Arguments : int numDimensions
// const int *size
// Return Type : emxArray_real_T *
//
emxArray_real_T *emxCreateND_real_T(int numDimensions, const int *size)
{
emxArray_real_T *emx;
int numEl;
int i;
emxInit_real_T(&emx, numDimensions);
numEl = 1;
for (i = 0; i < numDimensions; i++) {
numEl *= size[i];
emx->size[i] = size[i];
}
emx->data = (double *)std::calloc(static_cast<unsigned int>(numEl), sizeof
(double));
emx->numDimensions = numDimensions;
emx->allocatedSize = numEl;
return emx;
}
//
// Arguments : double *data
// int numDimensions
// const int *size
// Return Type : emxArray_real_T *
//
emxArray_real_T *emxCreateWrapperND_real_T(double *data, int numDimensions,
const int *size)
{
emxArray_real_T *emx;
int numEl;
int i;
emxInit_real_T(&emx, numDimensions);
numEl = 1;
for (i = 0; i < numDimensions; i++) {
numEl *= size[i];
emx->size[i] = size[i];
}
emx->data = data;
emx->numDimensions = numDimensions;
emx->allocatedSize = numEl;
emx->canFreeData = false;
return emx;
}
//
// Arguments : double *data
// int rows
// int cols
// Return Type : emxArray_real_T *
//
emxArray_real_T *emxCreateWrapper_real_T(double *data, int rows, int cols)
{
emxArray_real_T *emx;
emxInit_real_T(&emx, 2);
emx->size[0] = rows;
emx->size[1] = cols;
emx->data = data;
emx->numDimensions = 2;
emx->allocatedSize = rows * cols;
emx->canFreeData = false;
return emx;
}
//
// Arguments : int rows
// int cols
// Return Type : emxArray_real_T *
//
emxArray_real_T *emxCreate_real_T(int rows, int cols)
{
emxArray_real_T *emx;
int numEl;
emxInit_real_T(&emx, 2);
emx->size[0] = rows;
numEl = rows * cols;
emx->size[1] = cols;
emx->data = (double *)std::calloc(static_cast<unsigned int>(numEl), sizeof
(double));
emx->numDimensions = 2;
emx->allocatedSize = numEl;
return emx;
}
//
// Arguments : emxArray_real_T *emxArray
// Return Type : void
//
void emxDestroyArray_real_T(emxArray_real_T *emxArray)
{
emxFree_real_T(&emxArray);
}
//
// Arguments : emxArray_real_T **pEmxArray
// int numDimensions
// Return Type : void
//
void emxInitArray_real_T(emxArray_real_T **pEmxArray, int numDimensions)
{
emxInit_real_T(pEmxArray, numDimensions);
}
//
// File trailer for cwt_emxAPI.cpp
//
// [EOF]
//

32
Classes/cwt_emxAPI.h Normal file
View File

@@ -0,0 +1,32 @@
//
// File: cwt_emxAPI.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef CWT_EMXAPI_H
#define CWT_EMXAPI_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern emxArray_real_T *emxCreateND_real_T(int numDimensions, const int *size);
extern emxArray_real_T *emxCreateWrapperND_real_T(double *data, int
numDimensions, const int *size);
extern emxArray_real_T *emxCreateWrapper_real_T(double *data, int rows, int cols);
extern emxArray_real_T *emxCreate_real_T(int rows, int cols);
extern void emxDestroyArray_real_T(emxArray_real_T *emxArray);
extern void emxInitArray_real_T(emxArray_real_T **pEmxArray, int numDimensions);
#endif
//
// File trailer for cwt_emxAPI.h
//
// [EOF]
//

301
Classes/cwt_emxutil.cpp Normal file
View File

@@ -0,0 +1,301 @@
//
// File: cwt_emxutil.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "cwt_emxutil.h"
#include "cwt.h"
#include "rt_nonfinite.h"
#include <cstdlib>
#include <cstring>
#include <string.h>
// Function Definitions
//
// Arguments : emxArray_char_T *emxArray
// int oldNumel
// Return Type : void
//
void emxEnsureCapacity_char_T(emxArray_char_T *emxArray, int oldNumel)
{
int newNumel;
int i;
void *newData;
if (oldNumel < 0) {
oldNumel = 0;
}
newNumel = 1;
for (i = 0; i < emxArray->numDimensions; i++) {
newNumel *= emxArray->size[i];
}
if (newNumel > emxArray->allocatedSize) {
i = emxArray->allocatedSize;
if (i < 16) {
i = 16;
}
while (i < newNumel) {
if (i > 1073741823) {
i = MAX_int32_T;
} else {
i *= 2;
}
}
newData = std::calloc(static_cast<unsigned int>(i), sizeof(char));
if (emxArray->data != NULL) {
std::memcpy(newData, emxArray->data, sizeof(char) * oldNumel);
if (emxArray->canFreeData) {
std::free(emxArray->data);
}
}
emxArray->data = (char *)newData;
emxArray->allocatedSize = i;
emxArray->canFreeData = true;
}
}
//
// Arguments : emxArray_creal_T *emxArray
// int oldNumel
// Return Type : void
//
void emxEnsureCapacity_creal_T(emxArray_creal_T *emxArray, int oldNumel)
{
int newNumel;
int i;
void *newData;
if (oldNumel < 0) {
oldNumel = 0;
}
newNumel = 1;
for (i = 0; i < emxArray->numDimensions; i++) {
newNumel *= emxArray->size[i];
}
if (newNumel > emxArray->allocatedSize) {
i = emxArray->allocatedSize;
if (i < 16) {
i = 16;
}
while (i < newNumel) {
if (i > 1073741823) {
i = MAX_int32_T;
} else {
i *= 2;
}
}
newData = std::calloc(static_cast<unsigned int>(i), sizeof(creal_T));
if (emxArray->data != NULL) {
std::memcpy(newData, emxArray->data, sizeof(creal_T) * oldNumel);
if (emxArray->canFreeData) {
std::free(emxArray->data);
}
}
emxArray->data = (creal_T *)newData;
emxArray->allocatedSize = i;
emxArray->canFreeData = true;
}
}
//
// Arguments : emxArray_real_T *emxArray
// int oldNumel
// Return Type : void
//
void emxEnsureCapacity_real_T(emxArray_real_T *emxArray, int oldNumel)
{
int newNumel;
int i;
void *newData;
if (oldNumel < 0) {
oldNumel = 0;
}
newNumel = 1;
for (i = 0; i < emxArray->numDimensions; i++) {
newNumel *= emxArray->size[i];
}
if (newNumel > emxArray->allocatedSize) {
i = emxArray->allocatedSize;
if (i < 16) {
i = 16;
}
while (i < newNumel) {
if (i > 1073741823) {
i = MAX_int32_T;
} else {
i *= 2;
}
}
newData = std::calloc(static_cast<unsigned int>(i), sizeof(double));
if (emxArray->data != NULL) {
std::memcpy(newData, emxArray->data, sizeof(double) * oldNumel);
if (emxArray->canFreeData) {
std::free(emxArray->data);
}
}
emxArray->data = (double *)newData;
emxArray->allocatedSize = i;
emxArray->canFreeData = true;
}
}
//
// Arguments : cwtfilterbank *pStruct
// Return Type : void
//
void emxFreeStruct_cwtfilterbank(cwtfilterbank *pStruct)
{
emxFree_real_T(&pStruct->Scales);
emxFree_real_T(&pStruct->PsiDFT);
emxFree_real_T(&pStruct->WaveletCenterFrequencies);
emxFree_real_T(&pStruct->Omega);
}
//
// Arguments : emxArray_char_T **pEmxArray
// Return Type : void
//
void emxFree_char_T(emxArray_char_T **pEmxArray)
{
if (*pEmxArray != (emxArray_char_T *)NULL) {
if (((*pEmxArray)->data != (char *)NULL) && (*pEmxArray)->canFreeData) {
std::free((*pEmxArray)->data);
}
std::free((*pEmxArray)->size);
std::free(*pEmxArray);
*pEmxArray = (emxArray_char_T *)NULL;
}
}
//
// Arguments : emxArray_creal_T **pEmxArray
// Return Type : void
//
void emxFree_creal_T(emxArray_creal_T **pEmxArray)
{
if (*pEmxArray != (emxArray_creal_T *)NULL) {
if (((*pEmxArray)->data != (creal_T *)NULL) && (*pEmxArray)->canFreeData) {
std::free((*pEmxArray)->data);
}
std::free((*pEmxArray)->size);
std::free(*pEmxArray);
*pEmxArray = (emxArray_creal_T *)NULL;
}
}
//
// Arguments : emxArray_real_T **pEmxArray
// Return Type : void
//
void emxFree_real_T(emxArray_real_T **pEmxArray)
{
if (*pEmxArray != (emxArray_real_T *)NULL) {
if (((*pEmxArray)->data != (double *)NULL) && (*pEmxArray)->canFreeData) {
std::free((*pEmxArray)->data);
}
std::free((*pEmxArray)->size);
std::free(*pEmxArray);
*pEmxArray = (emxArray_real_T *)NULL;
}
}
//
// Arguments : cwtfilterbank *pStruct
// Return Type : void
//
void emxInitStruct_cwtfilterbank(cwtfilterbank *pStruct)
{
emxInit_real_T(&pStruct->Scales, 2);
emxInit_real_T(&pStruct->PsiDFT, 2);
emxInit_real_T(&pStruct->WaveletCenterFrequencies, 1);
emxInit_real_T(&pStruct->Omega, 2);
}
//
// Arguments : emxArray_char_T **pEmxArray
// int numDimensions
// Return Type : void
//
void emxInit_char_T(emxArray_char_T **pEmxArray, int numDimensions)
{
emxArray_char_T *emxArray;
int i;
*pEmxArray = (emxArray_char_T *)std::malloc(sizeof(emxArray_char_T));
emxArray = *pEmxArray;
emxArray->data = (char *)NULL;
emxArray->numDimensions = numDimensions;
emxArray->size = (int *)std::malloc(sizeof(int) * numDimensions);
emxArray->allocatedSize = 0;
emxArray->canFreeData = true;
for (i = 0; i < numDimensions; i++) {
emxArray->size[i] = 0;
}
}
//
// Arguments : emxArray_creal_T **pEmxArray
// int numDimensions
// Return Type : void
//
void emxInit_creal_T(emxArray_creal_T **pEmxArray, int numDimensions)
{
emxArray_creal_T *emxArray;
int i;
*pEmxArray = (emxArray_creal_T *)std::malloc(sizeof(emxArray_creal_T));
emxArray = *pEmxArray;
emxArray->data = (creal_T *)NULL;
emxArray->numDimensions = numDimensions;
emxArray->size = (int *)std::malloc(sizeof(int) * numDimensions);
emxArray->allocatedSize = 0;
emxArray->canFreeData = true;
for (i = 0; i < numDimensions; i++) {
emxArray->size[i] = 0;
}
}
//
// Arguments : emxArray_real_T **pEmxArray
// int numDimensions
// Return Type : void
//
void emxInit_real_T(emxArray_real_T **pEmxArray, int numDimensions)
{
emxArray_real_T *emxArray;
int i;
*pEmxArray = (emxArray_real_T *)std::malloc(sizeof(emxArray_real_T));
emxArray = *pEmxArray;
emxArray->data = (double *)NULL;
emxArray->numDimensions = numDimensions;
emxArray->size = (int *)std::malloc(sizeof(int) * numDimensions);
emxArray->allocatedSize = 0;
emxArray->canFreeData = true;
for (i = 0; i < numDimensions; i++) {
emxArray->size[i] = 0;
}
}
//
// File trailer for cwt_emxutil.cpp
//
// [EOF]
//

36
Classes/cwt_emxutil.h Normal file
View File

@@ -0,0 +1,36 @@
//
// File: cwt_emxutil.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef CWT_EMXUTIL_H
#define CWT_EMXUTIL_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern void emxEnsureCapacity_char_T(emxArray_char_T *emxArray, int oldNumel);
extern void emxEnsureCapacity_creal_T(emxArray_creal_T *emxArray, int oldNumel);
extern void emxEnsureCapacity_real_T(emxArray_real_T *emxArray, int oldNumel);
extern void emxFreeStruct_cwtfilterbank(cwtfilterbank *pStruct);
extern void emxFree_char_T(emxArray_char_T **pEmxArray);
extern void emxFree_creal_T(emxArray_creal_T **pEmxArray);
extern void emxFree_real_T(emxArray_real_T **pEmxArray);
extern void emxInitStruct_cwtfilterbank(cwtfilterbank *pStruct);
extern void emxInit_char_T(emxArray_char_T **pEmxArray, int numDimensions);
extern void emxInit_creal_T(emxArray_creal_T **pEmxArray, int numDimensions);
extern void emxInit_real_T(emxArray_real_T **pEmxArray, int numDimensions);
#endif
//
// File trailer for cwt_emxutil.h
//
// [EOF]
//

32
Classes/cwt_initialize.cpp Normal file
View File

@@ -0,0 +1,32 @@
//
// File: cwt_initialize.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "cwt_initialize.h"
#include "cwt.h"
#include "cwt_data.h"
#include "rt_nonfinite.h"
#include <string.h>
// Function Definitions
//
// Arguments : void
// Return Type : void
//
void cwt_initialize()
{
rt_InitInfAndNaN();
omp_init_nest_lock(&emlrtNestLockGlobal);
isInitialized_cwt = true;
}
//
// File trailer for cwt_initialize.cpp
//
// [EOF]
//

26
Classes/cwt_initialize.h Normal file
View File

@@ -0,0 +1,26 @@
//
// File: cwt_initialize.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef CWT_INITIALIZE_H
#define CWT_INITIALIZE_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern void cwt_initialize();
#endif
//
// File trailer for cwt_initialize.h
//
// [EOF]
//

73
Classes/cwt_rtwutil.cpp Normal file
View File

@@ -0,0 +1,73 @@
//
// File: cwt_rtwutil.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "cwt_rtwutil.h"
#include "cwt.h"
#include "rt_nonfinite.h"
#include <cmath>
#include <math.h>
#include <string.h>
// Function Definitions
//
// Arguments : double u0
// double u1
// Return Type : double
//
double rt_powd_snf(double u0, double u1)
{
double y;
double d;
double d1;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = rtNaN;
} else {
d = std::abs(u0);
d1 = std::abs(u1);
if (rtIsInf(u1)) {
if (d == 1.0) {
y = 1.0;
} else if (d > 1.0) {
if (u1 > 0.0) {
y = rtInf;
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = rtInf;
}
} else if (d1 == 0.0) {
y = 1.0;
} else if (d1 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = std::sqrt(u0);
} else if ((u0 < 0.0) && (u1 > std::floor(u1))) {
y = rtNaN;
} else {
y = pow(u0, u1);
}
}
return y;
}
//
// File trailer for cwt_rtwutil.cpp
//
// [EOF]
//

26
Classes/cwt_rtwutil.h Normal file
View File

@@ -0,0 +1,26 @@
//
// File: cwt_rtwutil.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef CWT_RTWUTIL_H
#define CWT_RTWUTIL_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern double rt_powd_snf(double u0, double u1);
#endif
//
// File trailer for cwt_rtwutil.h
//
// [EOF]
//

31
Classes/cwt_terminate.cpp Normal file
View File

@@ -0,0 +1,31 @@
//
// File: cwt_terminate.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "cwt_terminate.h"
#include "cwt.h"
#include "cwt_data.h"
#include "rt_nonfinite.h"
#include <string.h>
// Function Definitions
//
// Arguments : void
// Return Type : void
//
void cwt_terminate()
{
omp_destroy_nest_lock(&emlrtNestLockGlobal);
isInitialized_cwt = false;
}
//
// File trailer for cwt_terminate.cpp
//
// [EOF]
//

26
Classes/cwt_terminate.h Normal file
View File

@@ -0,0 +1,26 @@
//
// File: cwt_terminate.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef CWT_TERMINATE_H
#define CWT_TERMINATE_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern void cwt_terminate();
#endif
//
// File trailer for cwt_terminate.h
//
// [EOF]
//

67
Classes/cwt_types.h Normal file
View File

@@ -0,0 +1,67 @@
//
// File: cwt_types.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef CWT_TYPES_H
#define CWT_TYPES_H
// Include Files
#include "rtwtypes.h"
// Type Definitions
struct emxArray_real_T
{
double *data;
int *size;
int allocatedSize;
int numDimensions;
boolean_T canFreeData;
};
struct cwtfilterbank
{
double VoicesPerOctave;
char Wavelet[5];
double SamplingFrequency;
double SignalLength;
double FrequencyLimits[2];
double TimeBandwidth;
double WaveletParameters[2];
char Boundary[10];
emxArray_real_T *Scales;
emxArray_real_T *PsiDFT;
emxArray_real_T *WaveletCenterFrequencies;
double Beta;
double Gamma;
double SignalPad;
emxArray_real_T *Omega;
double CutOff;
};
struct emxArray_creal_T
{
creal_T *data;
int *size;
int allocatedSize;
int numDimensions;
boolean_T canFreeData;
};
struct emxArray_char_T
{
char *data;
int *size;
int allocatedSize;
int numDimensions;
boolean_T canFreeData;
};
#endif
//
// File trailer for cwt_types.h
//
// [EOF]
//

997
Classes/cwtfilterbank.cpp Normal file
View File

@@ -0,0 +1,997 @@
//
// File: cwtfilterbank.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "cwtfilterbank.h"
#include "bsxfun.h"
#include "cwt.h"
#include "cwt_data.h"
#include "cwt_emxutil.h"
#include "cwt_rtwutil.h"
#include "cwtfreqlimits.h"
#include "fft.h"
#include "ifft.h"
#include "log2.h"
#include "rt_nonfinite.h"
#include "wavCFandSD.h"
#include <cmath>
#include <cstring>
#include <stdio.h>
#include <string.h>
// Variable Definitions
static const char cv[10] = { 'r', 'e', 'f', 'l', 'e', 'c', 't', 'i', 'o', 'n' };
// Function Declarations
static void c_cwtfilterbank_validateInputsC(cwtfilterbank *self);
static void cwtfilterbank_filterbank(cwtfilterbank *self);
static int div_s32_floor(int numerator, int denominator);
// Function Definitions
//
// Arguments : cwtfilterbank *self
// Return Type : void
//
static void c_cwtfilterbank_validateInputsC(cwtfilterbank *self)
{
boolean_T guard1 = false;
boolean_T b[2];
boolean_T freqsep;
int ret;
boolean_T exitg1;
char a[10];
int exitg2;
double x;
double freqrange_idx_0;
double freqrange_idx_1;
double fs;
double ga;
double be;
double omegac;
char y[5];
char wav[5];
double FourierFactor;
double cf;
double unusedU10;
static const char b_b[5] = { 'm', 'o', 'r', 's', 'e' };
emxArray_char_T *charStr;
int nbytes;
guard1 = false;
if (!rtIsNaN(self->TimeBandwidth)) {
b[0] = rtIsNaN(self->WaveletParameters[0]);
b[1] = rtIsNaN(self->WaveletParameters[1]);
freqsep = true;
ret = 0;
exitg1 = false;
while ((!exitg1) && (ret < 2)) {
if (!b[ret]) {
freqsep = false;
exitg1 = true;
} else {
ret++;
}
}
if (freqsep) {
self->Beta = self->TimeBandwidth / self->Gamma;
} else {
guard1 = true;
}
} else {
guard1 = true;
}
if (guard1) {
b[0] = rtIsNaN(self->WaveletParameters[0]);
b[1] = rtIsNaN(self->WaveletParameters[1]);
freqsep = true;
ret = 0;
exitg1 = false;
while ((!exitg1) && (ret < 2)) {
if (!b[ret]) {
freqsep = false;
exitg1 = true;
} else {
ret++;
}
}
if ((!freqsep) && rtIsNaN(self->TimeBandwidth)) {
self->Gamma = self->WaveletParameters[0];
self->Beta = self->WaveletParameters[1] / self->Gamma;
}
}
for (ret = 0; ret < 10; ret++) {
a[ret] = self->Boundary[ret];
}
freqsep = false;
ret = 0;
do {
exitg2 = 0;
if (ret < 10) {
if (cv1[static_cast<unsigned char>(a[ret]) & 127] != cv1[static_cast<int>
(cv[ret])]) {
exitg2 = 1;
} else {
ret++;
}
} else {
freqsep = true;
exitg2 = 1;
}
} while (exitg2 == 0);
if (freqsep) {
if (self->SignalLength <= 100000.0) {
self->SignalPad = std::floor(self->SignalLength / 2.0);
} else {
x = b_log2(self->SignalLength);
self->SignalPad = std::ceil(x);
}
} else {
self->SignalPad = 0.0;
}
b[0] = rtIsNaN(self->FrequencyLimits[0]);
b[1] = rtIsNaN(self->FrequencyLimits[1]);
freqsep = true;
ret = 0;
exitg1 = false;
while ((!exitg1) && (ret < 2)) {
if (!b[ret]) {
freqsep = false;
exitg1 = true;
} else {
ret++;
}
}
if (!freqsep) {
freqrange_idx_0 = self->FrequencyLimits[0];
freqrange_idx_1 = self->FrequencyLimits[1];
fs = self->SamplingFrequency;
ga = self->Gamma;
be = self->Beta;
for (ret = 0; ret < 5; ret++) {
y[ret] = self->Wavelet[ret];
wav[ret] = cv1[static_cast<unsigned char>(y[ret]) & 127];
}
omegac = 3.1415926535897931;
wavCFandSD(wav, ga, be, &FourierFactor, &x, &cf);
unusedU10 = self->SignalLength / (x * 2.0);
ret = memcmp(&wav[0], &b_b[0], 5);
if (ret == 0) {
ret = 0;
} else {
ret = -1;
}
if (static_cast<int>((ret == 0))) {
omegac = getFreqFromCutoffMorse(self->CutOff / 100.0, cf, ga, be);
}
x = omegac / 3.1415926535897931 * rt_powd_snf(2.0, 1.0 /
self->VoicesPerOctave);
if (unusedU10 < x) {
unusedU10 = x;
}
x = 1.0 / (unusedU10 * FourierFactor) * self->SamplingFrequency;
if (freqrange_idx_0 < x) {
self->FrequencyLimits[0] = x;
freqrange_idx_0 = self->FrequencyLimits[0];
}
if (freqrange_idx_1 > fs / 2.0) {
self->FrequencyLimits[1] = fs / 2.0;
freqrange_idx_1 = self->FrequencyLimits[1];
}
freqsep = (b_log2(freqrange_idx_1) - b_log2(freqrange_idx_0) >= 1.0 /
self->VoicesPerOctave);
if (!freqsep) {
emxInit_char_T(&charStr, 2);
x = 1.0 / self->VoicesPerOctave;
nbytes = (int)snprintf(NULL, 0, "%2.2f", x) + 1;
ret = charStr->size[0] * charStr->size[1];
charStr->size[0] = 1;
charStr->size[1] = nbytes;
emxEnsureCapacity_char_T(charStr, ret);
snprintf(&charStr->data[0], (size_t)nbytes, "%2.2f", x);
emxFree_char_T(&charStr);
}
}
}
//
// Arguments : cwtfilterbank *self
// Return Type : void
//
static void cwtfilterbank_filterbank(cwtfilterbank *self)
{
int i;
boolean_T b_bool;
char a[5];
int csz_idx_1;
int exitg1;
static const char b_cv[5] = { 'M', 'o', 'r', 's', 'e' };
emxArray_real_T *f;
emxArray_real_T *omega;
emxArray_real_T *absomega;
double ga;
double be;
emxArray_real_T *somega;
emxArray_real_T *c;
int bcoef;
int loop_ub;
double fo;
emxArray_real_T *powscales;
unsigned int unnamed_idx_0;
unsigned int unnamed_idx_1;
for (i = 0; i < 5; i++) {
a[i] = self->Wavelet[i];
}
b_bool = false;
csz_idx_1 = 0;
do {
exitg1 = 0;
if (csz_idx_1 < 5) {
if (cv1[static_cast<unsigned char>(a[csz_idx_1]) & 127] != cv1[
static_cast<int>(b_cv[csz_idx_1])]) {
exitg1 = 1;
} else {
csz_idx_1++;
}
} else {
b_bool = true;
exitg1 = 1;
}
} while (exitg1 == 0);
emxInit_real_T(&f, 2);
emxInit_real_T(&omega, 2);
emxInit_real_T(&absomega, 2);
if (b_bool) {
i = omega->size[0] * omega->size[1];
omega->size[0] = 1;
omega->size[1] = self->Omega->size[1];
emxEnsureCapacity_real_T(omega, i);
csz_idx_1 = self->Omega->size[0] * self->Omega->size[1];
for (i = 0; i < csz_idx_1; i++) {
omega->data[i] = self->Omega->data[i];
}
i = f->size[0] * f->size[1];
f->size[0] = 1;
f->size[1] = self->Scales->size[1];
emxEnsureCapacity_real_T(f, i);
csz_idx_1 = self->Scales->size[0] * self->Scales->size[1];
for (i = 0; i < csz_idx_1; i++) {
f->data[i] = self->Scales->data[i];
}
ga = self->Gamma;
be = self->Beta;
emxInit_real_T(&somega, 2);
if (f->size[1] == 1) {
emxInit_real_T(&c, 2);
if (omega->size[1] == 1) {
csz_idx_1 = 1;
} else {
csz_idx_1 = omega->size[1];
}
i = c->size[0] * c->size[1];
c->size[0] = 1;
if (omega->size[1] == 1) {
c->size[1] = 1;
} else {
c->size[1] = omega->size[1];
}
emxEnsureCapacity_real_T(c, i);
if (csz_idx_1 != 0) {
bcoef = (omega->size[1] != 1);
i = csz_idx_1 - 1;
for (loop_ub = 0; loop_ub <= i; loop_ub++) {
c->data[loop_ub] = f->data[0] * omega->data[bcoef * loop_ub];
}
}
i = somega->size[0] * somega->size[1];
somega->size[0] = 1;
somega->size[1] = c->size[1];
emxEnsureCapacity_real_T(somega, i);
csz_idx_1 = c->size[0] * c->size[1];
for (i = 0; i < csz_idx_1; i++) {
somega->data[i] = c->data[i];
}
emxFree_real_T(&c);
} else {
i = somega->size[0] * somega->size[1];
somega->size[0] = f->size[1];
somega->size[1] = omega->size[1];
emxEnsureCapacity_real_T(somega, i);
csz_idx_1 = omega->size[1];
for (i = 0; i < csz_idx_1; i++) {
loop_ub = f->size[1];
for (bcoef = 0; bcoef < loop_ub; bcoef++) {
somega->data[bcoef + somega->size[0] * i] = f->data[bcoef] *
omega->data[i];
}
}
}
fo = std::exp(1.0 / ga * (std::log(be) - std::log(ga)));
csz_idx_1 = somega->size[0] * somega->size[1];
i = absomega->size[0] * absomega->size[1];
absomega->size[0] = somega->size[0];
absomega->size[1] = somega->size[1];
emxEnsureCapacity_real_T(absomega, i);
for (loop_ub = 0; loop_ub < csz_idx_1; loop_ub++) {
absomega->data[loop_ub] = std::abs(somega->data[loop_ub]);
}
emxInit_real_T(&powscales, 2);
if (ga == 3.0) {
i = powscales->size[0] * powscales->size[1];
powscales->size[0] = absomega->size[0];
powscales->size[1] = absomega->size[1];
emxEnsureCapacity_real_T(powscales, i);
csz_idx_1 = absomega->size[0] * absomega->size[1];
for (i = 0; i < csz_idx_1; i++) {
powscales->data[i] = absomega->data[i] * absomega->data[i] *
absomega->data[i];
}
} else {
unnamed_idx_0 = static_cast<unsigned int>(absomega->size[0]);
unnamed_idx_1 = static_cast<unsigned int>(absomega->size[1]);
i = powscales->size[0] * powscales->size[1];
powscales->size[0] = static_cast<int>(unnamed_idx_0);
powscales->size[1] = static_cast<int>(unnamed_idx_1);
emxEnsureCapacity_real_T(powscales, i);
csz_idx_1 = static_cast<int>(unnamed_idx_0) * static_cast<int>
(unnamed_idx_1);
for (loop_ub = 0; loop_ub < csz_idx_1; loop_ub++) {
powscales->data[loop_ub] = rt_powd_snf(absomega->data[loop_ub], ga);
}
}
csz_idx_1 = absomega->size[0] * absomega->size[1];
for (loop_ub = 0; loop_ub < csz_idx_1; loop_ub++) {
absomega->data[loop_ub] = std::log(absomega->data[loop_ub]);
}
ga = 2.0 * std::exp(-be * std::log(fo) + rt_powd_snf(fo, ga));
csz_idx_1 = absomega->size[0] * absomega->size[1];
for (i = 0; i < csz_idx_1; i++) {
absomega->data[i] = be * absomega->data[i] - powscales->data[i];
}
emxFree_real_T(&powscales);
csz_idx_1 = absomega->size[0] * absomega->size[1];
for (loop_ub = 0; loop_ub < csz_idx_1; loop_ub++) {
absomega->data[loop_ub] = std::exp(absomega->data[loop_ub]);
}
csz_idx_1 = absomega->size[0] * absomega->size[1];
for (i = 0; i < csz_idx_1; i++) {
absomega->data[i] = ga * absomega->data[i] * static_cast<double>
((somega->data[i] > 0.0));
}
emxFree_real_T(&somega);
i = f->size[0] * f->size[1];
bcoef = f->size[0] * f->size[1];
f->size[0] = 1;
emxEnsureCapacity_real_T(f, bcoef);
csz_idx_1 = i - 1;
for (i = 0; i <= csz_idx_1; i++) {
f->data[i] = fo / f->data[i] / 6.2831853071795862;
}
} else {
i = omega->size[0] * omega->size[1];
omega->size[0] = 1;
omega->size[1] = self->Omega->size[1];
emxEnsureCapacity_real_T(omega, i);
csz_idx_1 = self->Omega->size[0] * self->Omega->size[1];
for (i = 0; i < csz_idx_1; i++) {
omega->data[i] = self->Omega->data[i];
}
i = f->size[0] * f->size[1];
f->size[0] = 1;
f->size[1] = self->Scales->size[1];
emxEnsureCapacity_real_T(f, i);
csz_idx_1 = self->Scales->size[0] * self->Scales->size[1];
for (i = 0; i < csz_idx_1; i++) {
f->data[i] = self->Scales->data[i];
}
i = absomega->size[0] * absomega->size[1];
absomega->size[0] = f->size[1];
absomega->size[1] = omega->size[1];
emxEnsureCapacity_real_T(absomega, i);
csz_idx_1 = f->size[1] * omega->size[1];
for (i = 0; i < csz_idx_1; i++) {
absomega->data[i] = 0.0;
}
i = f->size[0] * f->size[1];
bcoef = f->size[0] * f->size[1];
f->size[0] = 1;
emxEnsureCapacity_real_T(f, bcoef);
csz_idx_1 = i - 1;
for (i = 0; i <= csz_idx_1; i++) {
f->data[i] = 0.0 / f->data[i];
}
}
emxFree_real_T(&omega);
i = f->size[0] * f->size[1];
bcoef = f->size[0] * f->size[1];
f->size[0] = 1;
emxEnsureCapacity_real_T(f, bcoef);
csz_idx_1 = i - 1;
for (i = 0; i <= csz_idx_1; i++) {
f->data[i] *= self->SamplingFrequency;
}
i = self->PsiDFT->size[0] * self->PsiDFT->size[1];
self->PsiDFT->size[0] = absomega->size[0];
self->PsiDFT->size[1] = absomega->size[1];
emxEnsureCapacity_real_T(self->PsiDFT, i);
csz_idx_1 = absomega->size[0] * absomega->size[1];
for (i = 0; i < csz_idx_1; i++) {
self->PsiDFT->data[i] = absomega->data[i];
}
emxFree_real_T(&absomega);
i = self->WaveletCenterFrequencies->size[0];
self->WaveletCenterFrequencies->size[0] = f->size[1];
emxEnsureCapacity_real_T(self->WaveletCenterFrequencies, i);
csz_idx_1 = f->size[1];
for (i = 0; i < csz_idx_1; i++) {
self->WaveletCenterFrequencies->data[i] = f->data[i];
}
emxFree_real_T(&f);
}
//
// Arguments : int numerator
// int denominator
// Return Type : int
//
static int div_s32_floor(int numerator, int denominator)
{
int quotient;
unsigned int absNumerator;
unsigned int absDenominator;
boolean_T quotientNeedsNegation;
unsigned int tempAbsQuotient;
if (denominator == 0) {
if (numerator >= 0) {
quotient = MAX_int32_T;
} else {
quotient = MIN_int32_T;
}
} else {
if (numerator < 0) {
absNumerator = ~static_cast<unsigned int>(numerator) + 1U;
} else {
absNumerator = static_cast<unsigned int>(numerator);
}
if (denominator < 0) {
absDenominator = ~static_cast<unsigned int>(denominator) + 1U;
} else {
absDenominator = static_cast<unsigned int>(denominator);
}
quotientNeedsNegation = ((numerator < 0) != (denominator < 0));
tempAbsQuotient = absNumerator / absDenominator;
if (quotientNeedsNegation) {
absNumerator %= absDenominator;
if (absNumerator > 0U) {
tempAbsQuotient++;
}
quotient = -static_cast<int>(tempAbsQuotient);
} else {
quotient = static_cast<int>(tempAbsQuotient);
}
}
return quotient;
}
//
// Arguments : cwtfilterbank *self
// double varargin_2
// Return Type : cwtfilterbank *
//
cwtfilterbank *cwtfilterbank_cwtfilterbank(cwtfilterbank *self, double
varargin_2)
{
cwtfilterbank *b_self;
int nx;
static const char b_cv[5] = { 'M', 'o', 'r', 's', 'e' };
double frange_idx_1;
boolean_T b_bool;
char a[5];
int ret;
int exitg1;
static const char b_cv1[5] = { 'M', 'o', 'r', 's', 'e' };
double N;
emxArray_real_T *omega;
int loop_ub;
int i;
int i1;
emxArray_real_T *y;
double frange_idx_0;
boolean_T b[2];
boolean_T exitg2;
char x[5];
double be;
double nv;
double cutoff;
double FourierFactor;
double omegac;
double cf;
static const char b_b[5] = { 'm', 'o', 'r', 's', 'e' };
b_self = self;
b_self->CutOff = 50.0;
b_self->Gamma = 3.0;
b_self->Beta = 20.0;
for (nx = 0; nx < 5; nx++) {
b_self->Wavelet[nx] = b_cv[nx];
}
b_self->TimeBandwidth = rtNaN;
b_self->WaveletParameters[0] = rtNaN;
b_self->WaveletParameters[1] = rtNaN;
b_self->SignalLength = varargin_2;
frange_idx_1 = b_self->SignalLength / 2.0;
frange_idx_1 = std::floor(frange_idx_1);
b_self->SignalPad = frange_idx_1;
b_self->VoicesPerOctave = 10.0;
b_self->SamplingFrequency = 1.0;
b_self->FrequencyLimits[0] = rtNaN;
b_self->FrequencyLimits[1] = rtNaN;
for (nx = 0; nx < 10; nx++) {
b_self->Boundary[nx] = cv[nx];
}
c_cwtfilterbank_validateInputsC(b_self);
for (nx = 0; nx < 5; nx++) {
a[nx] = b_self->Wavelet[nx];
}
b_bool = false;
ret = 0;
do {
exitg1 = 0;
if (ret < 5) {
if (cv1[static_cast<unsigned char>(a[ret]) & 127] != cv1[static_cast<int>
(b_cv1[ret])]) {
exitg1 = 1;
} else {
ret++;
}
} else {
b_bool = true;
exitg1 = 1;
}
} while (exitg1 == 0);
if (!b_bool) {
b_self->CutOff = 10.0;
}
N = b_self->SignalLength + 2.0 * b_self->SignalPad;
frange_idx_1 = N / 2.0;
if (frange_idx_1 < 0.0) {
frange_idx_1 = std::ceil(frange_idx_1);
} else {
frange_idx_1 = std::floor(frange_idx_1);
}
emxInit_real_T(&omega, 2);
if (rtIsNaN(frange_idx_1)) {
nx = omega->size[0] * omega->size[1];
omega->size[0] = 1;
omega->size[1] = 1;
emxEnsureCapacity_real_T(omega, nx);
omega->data[0] = rtNaN;
} else if (frange_idx_1 < 1.0) {
omega->size[0] = 1;
omega->size[1] = 0;
} else if (rtIsInf(frange_idx_1) && (1.0 == frange_idx_1)) {
nx = omega->size[0] * omega->size[1];
omega->size[0] = 1;
omega->size[1] = 1;
emxEnsureCapacity_real_T(omega, nx);
omega->data[0] = rtNaN;
} else {
nx = omega->size[0] * omega->size[1];
omega->size[0] = 1;
omega->size[1] = static_cast<int>((frange_idx_1 - 1.0)) + 1;
emxEnsureCapacity_real_T(omega, nx);
loop_ub = static_cast<int>((frange_idx_1 - 1.0));
for (nx = 0; nx <= loop_ub; nx++) {
omega->data[nx] = static_cast<double>(nx) + 1.0;
}
}
nx = omega->size[0] * omega->size[1];
i = omega->size[0] * omega->size[1];
omega->size[0] = 1;
emxEnsureCapacity_real_T(omega, i);
loop_ub = nx - 1;
for (nx = 0; nx <= loop_ub; nx++) {
omega->data[nx] = omega->data[nx] * 6.2831853071795862 / N;
}
frange_idx_1 = (N - 1.0) / 2.0;
if (frange_idx_1 < 0.0) {
frange_idx_1 = std::ceil(frange_idx_1);
} else {
frange_idx_1 = std::floor(frange_idx_1);
}
if (1.0 > frange_idx_1) {
nx = 0;
i = 1;
i1 = -1;
} else {
nx = static_cast<int>(frange_idx_1) - 1;
i = -1;
i1 = 0;
}
emxInit_real_T(&y, 2);
ret = y->size[0] * y->size[1];
y->size[0] = 1;
loop_ub = div_s32_floor(i1 - nx, i);
y->size[1] = (omega->size[1] + loop_ub) + 2;
emxEnsureCapacity_real_T(y, ret);
y->data[0] = 0.0;
ret = omega->size[1];
for (i1 = 0; i1 < ret; i1++) {
y->data[i1 + 1] = omega->data[i1];
}
for (i1 = 0; i1 <= loop_ub; i1++) {
y->data[(i1 + omega->size[1]) + 1] = -omega->data[nx + i * i1];
}
nx = omega->size[0] * omega->size[1];
omega->size[0] = 1;
omega->size[1] = y->size[1];
emxEnsureCapacity_real_T(omega, nx);
loop_ub = y->size[0] * y->size[1];
for (nx = 0; nx < loop_ub; nx++) {
omega->data[nx] = y->data[nx];
}
nx = b_self->Omega->size[0] * b_self->Omega->size[1];
b_self->Omega->size[0] = 1;
b_self->Omega->size[1] = omega->size[1];
emxEnsureCapacity_real_T(b_self->Omega, nx);
loop_ub = omega->size[0] * omega->size[1];
for (nx = 0; nx < loop_ub; nx++) {
b_self->Omega->data[nx] = omega->data[nx];
}
frange_idx_0 = b_self->FrequencyLimits[0];
frange_idx_1 = b_self->FrequencyLimits[1];
b[0] = rtIsNaN(frange_idx_0);
b[1] = rtIsNaN(frange_idx_1);
b_bool = true;
ret = 0;
exitg2 = false;
while ((!exitg2) && (ret < 2)) {
if (!b[ret]) {
b_bool = false;
exitg2 = true;
} else {
ret++;
}
}
if (!b_bool) {
frange_idx_0 = b_self->FrequencyLimits[0];
frange_idx_1 = b_self->FrequencyLimits[1];
frange_idx_0 /= b_self->SamplingFrequency;
frange_idx_1 /= b_self->SamplingFrequency;
frange_idx_0 = frange_idx_0 * 2.0 * 3.1415926535897931;
frange_idx_1 = frange_idx_1 * 2.0 * 3.1415926535897931;
nv = b_self->VoicesPerOctave;
wavCFandSD(b_self->Wavelet, b_self->Gamma, b_self->Beta, &FourierFactor,
&cutoff, &be);
omegac = be / frange_idx_1;
frange_idx_1 = b_log2(be / frange_idx_0 / omegac);
frange_idx_1 *= nv;
if (rtIsNaN(frange_idx_1)) {
nx = omega->size[0] * omega->size[1];
omega->size[0] = 1;
omega->size[1] = 1;
emxEnsureCapacity_real_T(omega, nx);
omega->data[0] = rtNaN;
} else if (frange_idx_1 < 0.0) {
omega->size[0] = 1;
omega->size[1] = 0;
} else if (rtIsInf(frange_idx_1) && (0.0 == frange_idx_1)) {
nx = omega->size[0] * omega->size[1];
omega->size[0] = 1;
omega->size[1] = 1;
emxEnsureCapacity_real_T(omega, nx);
omega->data[0] = rtNaN;
} else {
nx = omega->size[0] * omega->size[1];
omega->size[0] = 1;
loop_ub = static_cast<int>(std::floor(frange_idx_1));
omega->size[1] = loop_ub + 1;
emxEnsureCapacity_real_T(omega, nx);
for (nx = 0; nx <= loop_ub; nx++) {
omega->data[nx] = nx;
}
}
cutoff = rt_powd_snf(2.0, 1.0 / nv);
nx = y->size[0] * y->size[1];
y->size[0] = 1;
y->size[1] = omega->size[1];
emxEnsureCapacity_real_T(y, nx);
nx = omega->size[1];
for (ret = 0; ret < nx; ret++) {
y->data[ret] = rt_powd_snf(cutoff, omega->data[ret]);
}
nx = b_self->Scales->size[0] * b_self->Scales->size[1];
b_self->Scales->size[0] = 1;
b_self->Scales->size[1] = y->size[1];
emxEnsureCapacity_real_T(b_self->Scales, nx);
loop_ub = y->size[0] * y->size[1];
for (nx = 0; nx < loop_ub; nx++) {
b_self->Scales->data[nx] = omegac * y->data[nx];
}
} else {
for (nx = 0; nx < 5; nx++) {
x[nx] = b_self->Wavelet[nx];
}
N = b_self->SignalLength;
frange_idx_0 = b_self->Gamma;
be = b_self->Beta;
nv = b_self->VoicesPerOctave;
cutoff = b_self->CutOff;
for (ret = 0; ret < 5; ret++) {
a[ret] = cv1[static_cast<unsigned char>(x[ret]) & 127];
}
omegac = 3.1415926535897931;
cutoff /= 100.0;
wavCFandSD(a, frange_idx_0, be, &FourierFactor, &frange_idx_1, &cf);
FourierFactor = N / (frange_idx_1 * 2.0);
ret = memcmp(&a[0], &b_b[0], 5);
if (ret == 0) {
ret = 0;
} else {
ret = -1;
}
if (static_cast<int>((ret == 0))) {
omegac = getFreqFromCutoffMorse(cutoff, cf, frange_idx_0, be);
}
omegac /= 3.1415926535897931;
cutoff = rt_powd_snf(2.0, 1.0 / nv);
frange_idx_1 = omegac * cutoff;
if (FourierFactor < frange_idx_1) {
FourierFactor = frange_idx_1;
}
frange_idx_1 = b_log2(FourierFactor / omegac);
FourierFactor = 1.0 / nv;
if ((frange_idx_1 > FourierFactor) || rtIsNaN(FourierFactor)) {
FourierFactor = frange_idx_1;
}
frange_idx_1 = FourierFactor * nv;
if (rtIsNaN(frange_idx_1)) {
nx = omega->size[0] * omega->size[1];
omega->size[0] = 1;
omega->size[1] = 1;
emxEnsureCapacity_real_T(omega, nx);
omega->data[0] = rtNaN;
} else if (frange_idx_1 < 0.0) {
omega->size[0] = 1;
omega->size[1] = 0;
} else if (rtIsInf(frange_idx_1) && (0.0 == frange_idx_1)) {
nx = omega->size[0] * omega->size[1];
omega->size[0] = 1;
omega->size[1] = 1;
emxEnsureCapacity_real_T(omega, nx);
omega->data[0] = rtNaN;
} else {
nx = omega->size[0] * omega->size[1];
omega->size[0] = 1;
loop_ub = static_cast<int>(std::floor(frange_idx_1));
omega->size[1] = loop_ub + 1;
emxEnsureCapacity_real_T(omega, nx);
for (nx = 0; nx <= loop_ub; nx++) {
omega->data[nx] = nx;
}
}
nx = y->size[0] * y->size[1];
y->size[0] = 1;
y->size[1] = omega->size[1];
emxEnsureCapacity_real_T(y, nx);
nx = omega->size[1];
for (ret = 0; ret < nx; ret++) {
y->data[ret] = rt_powd_snf(cutoff, omega->data[ret]);
}
nx = b_self->Scales->size[0] * b_self->Scales->size[1];
b_self->Scales->size[0] = 1;
b_self->Scales->size[1] = y->size[1];
emxEnsureCapacity_real_T(b_self->Scales, nx);
loop_ub = y->size[0] * y->size[1];
for (nx = 0; nx < loop_ub; nx++) {
b_self->Scales->data[nx] = omegac * y->data[nx];
}
}
emxFree_real_T(&y);
emxFree_real_T(&omega);
cwtfilterbank_filterbank(b_self);
return b_self;
}
//
// Arguments : const cwtfilterbank *self
// double x_data[]
// const int x_size[2]
// emxArray_creal_T *varargout_1
// Return Type : void
//
void cwtfilterbank_wt(const cwtfilterbank *self, double x_data[], const int
x_size[2], emxArray_creal_T *varargout_1)
{
int nd2;
int loop_ub;
double b_x_data[1280];
int xv_size[2];
double xv_data[3841];
double xtmp;
emxArray_creal_T *cfspos;
emxArray_creal_T *r;
static creal_T tmp_data[3841];
int tmp_size[2];
int i;
int i1;
int i2;
int b_j1;
int j2;
nd2 = x_size[1];
loop_ub = x_size[1];
if (0 <= loop_ub - 1) {
std::memcpy(&b_x_data[0], &x_data[0], loop_ub * sizeof(double));
}
if (0 <= nd2 - 1) {
std::memcpy(&x_data[0], &b_x_data[0], nd2 * sizeof(double));
}
xv_size[0] = 1;
xv_size[1] = nd2;
if (0 <= nd2 - 1) {
std::memcpy(&xv_data[0], &x_data[0], nd2 * sizeof(double));
}
if (self->SignalPad > 0.0) {
xtmp = self->SignalPad;
if (1.0 > xtmp) {
loop_ub = 0;
} else {
loop_ub = static_cast<int>(xtmp);
}
xtmp = (static_cast<double>(nd2) - self->SignalPad) + 1.0;
if (xtmp > nd2) {
i = 0;
i1 = 1;
i2 = -1;
} else {
i = nd2 - 1;
i1 = -1;
i2 = static_cast<int>(xtmp) - 1;
}
if (0 <= loop_ub - 1) {
std::memcpy(&b_x_data[0], &x_data[0], loop_ub * sizeof(double));
}
nd2 = loop_ub >> 1;
for (b_j1 = 0; b_j1 < nd2; b_j1++) {
j2 = (static_cast<short>(loop_ub) - b_j1) - 1;
xtmp = b_x_data[b_j1];
b_x_data[b_j1] = b_x_data[j2];
b_x_data[j2] = xtmp;
}
xv_size[0] = 1;
nd2 = div_s32_floor(i2 - i, i1);
xv_size[1] = ((loop_ub + x_size[1]) + nd2) + 1;
if (0 <= loop_ub - 1) {
std::memcpy(&xv_data[0], &b_x_data[0], loop_ub * sizeof(double));
}
b_j1 = x_size[1];
for (i2 = 0; i2 < b_j1; i2++) {
xv_data[i2 + loop_ub] = x_data[i2];
}
for (i2 = 0; i2 <= nd2; i2++) {
xv_data[(i2 + loop_ub) + x_size[1]] = x_data[i + i1 * i2];
}
}
emxInit_creal_T(&cfspos, 2);
emxInit_creal_T(&r, 2);
fft(xv_data, xv_size, tmp_data, tmp_size);
bsxfun(tmp_data, tmp_size, self->PsiDFT, r);
ifft(r, cfspos);
i = varargout_1->size[0] * varargout_1->size[1];
varargout_1->size[0] = cfspos->size[0];
varargout_1->size[1] = cfspos->size[1];
emxEnsureCapacity_creal_T(varargout_1, i);
loop_ub = cfspos->size[0] * cfspos->size[1];
emxFree_creal_T(&r);
for (i = 0; i < loop_ub; i++) {
varargout_1->data[i] = cfspos->data[i];
}
if (self->SignalPad > 0.0) {
xtmp = self->SignalPad;
loop_ub = cfspos->size[0];
i = varargout_1->size[0] * varargout_1->size[1];
varargout_1->size[0] = cfspos->size[0];
nd2 = static_cast<int>(std::floor(self->SignalLength - 1.0));
varargout_1->size[1] = nd2 + 1;
emxEnsureCapacity_creal_T(varargout_1, i);
for (i = 0; i <= nd2; i++) {
for (i1 = 0; i1 < loop_ub; i1++) {
varargout_1->data[i1 + varargout_1->size[0] * i] = cfspos->data[i1 +
cfspos->size[0] * (static_cast<int>((xtmp + static_cast<double>((i + 1))))
- 1)];
}
}
}
emxFree_creal_T(&cfspos);
}
//
// File trailer for cwtfilterbank.cpp
//
// [EOF]
//

29
Classes/cwtfilterbank.h Normal file
View File

@@ -0,0 +1,29 @@
//
// File: cwtfilterbank.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef CWTFILTERBANK_H
#define CWTFILTERBANK_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern cwtfilterbank *cwtfilterbank_cwtfilterbank(cwtfilterbank *self, double
varargin_2);
extern void cwtfilterbank_wt(const cwtfilterbank *self, double x_data[], const
int x_size[2], emxArray_creal_T *varargout_1);
#endif
//
// File trailer for cwtfilterbank.h
//
// [EOF]
//

149
Classes/cwtfreqlimits.cpp Normal file
View File

@@ -0,0 +1,149 @@
//
// File: cwtfreqlimits.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "cwtfreqlimits.h"
#include "cwt.h"
#include "cwt_rtwutil.h"
#include "cwtfilterbank.h"
#include "rt_nonfinite.h"
#include <cmath>
#include <string.h>
// Function Definitions
//
// Arguments : double cutoff
// double cf
// double ga
// double be
// Return Type : double
//
double getFreqFromCutoffMorse(double cutoff, double cf, double ga, double be)
{
double omegac;
double psihat_tunableEnvironment_idx_1;
double psihat_tunableEnvironment_idx_0;
double fa;
double a;
double fb;
double fc;
double c;
double e;
double d;
boolean_T exitg1;
double m;
double p;
double toler;
double s;
double r;
psihat_tunableEnvironment_idx_1 = 2.0 * std::exp(be / ga * ((std::log(ga) -
std::log(be)) + 1.0));
psihat_tunableEnvironment_idx_0 = 2.0 * cutoff;
omegac = rt_powd_snf(750.0, 1.0 / ga);
fa = psihat_tunableEnvironment_idx_0 - psihat_tunableEnvironment_idx_1 *
rt_powd_snf(cf, be) * std::exp(-rt_powd_snf(cf, ga));
if (fa >= 0.0) {
if (!(psihat_tunableEnvironment_idx_0 - psihat_tunableEnvironment_idx_1 *
rt_powd_snf(omegac, be) * std::exp(-rt_powd_snf(omegac, ga)) ==
psihat_tunableEnvironment_idx_0 - psihat_tunableEnvironment_idx_1 *
rt_powd_snf(cf, be) * std::exp(-rt_powd_snf(cf, ga)))) {
omegac = cf;
}
} else {
a = cf;
fb = psihat_tunableEnvironment_idx_0 - psihat_tunableEnvironment_idx_1 *
rt_powd_snf(omegac, be) * std::exp(-rt_powd_snf(omegac, ga));
if (!(fb == 0.0)) {
fc = fb;
c = omegac;
e = 0.0;
d = 0.0;
exitg1 = false;
while ((!exitg1) && ((fb != 0.0) && (a != omegac))) {
if ((fb > 0.0) == (fc > 0.0)) {
c = a;
fc = fa;
d = omegac - a;
e = d;
}
if (std::abs(fc) < std::abs(fb)) {
a = omegac;
omegac = c;
c = a;
fa = fb;
fb = fc;
fc = fa;
}
m = 0.5 * (c - omegac);
p = std::abs(omegac);
if (!(p > 1.0)) {
p = 1.0;
}
toler = 4.4408920985006262E-16 * p;
if ((std::abs(m) <= toler) || (fb == 0.0)) {
exitg1 = true;
} else {
if ((std::abs(e) < toler) || (std::abs(fa) <= std::abs(fb))) {
d = m;
e = m;
} else {
s = fb / fa;
if (a == c) {
p = 2.0 * m * s;
fa = 1.0 - s;
} else {
fa /= fc;
r = fb / fc;
p = s * (2.0 * m * fa * (fa - r) - (omegac - a) * (r - 1.0));
fa = (fa - 1.0) * (r - 1.0) * (s - 1.0);
}
if (p > 0.0) {
fa = -fa;
} else {
p = -p;
}
if ((2.0 * p < 3.0 * m * fa - std::abs(toler * fa)) && (p < std::abs
(0.5 * e * fa))) {
e = d;
d = p / fa;
} else {
d = m;
e = m;
}
}
a = omegac;
fa = fb;
if (std::abs(d) > toler) {
omegac += d;
} else if (omegac > c) {
omegac -= toler;
} else {
omegac += toler;
}
fb = psihat_tunableEnvironment_idx_0 - psihat_tunableEnvironment_idx_1
* rt_powd_snf(omegac, be) * std::exp(-rt_powd_snf(omegac, ga));
}
}
}
}
return omegac;
}
//
// File trailer for cwtfreqlimits.cpp
//
// [EOF]
//

27
Classes/cwtfreqlimits.h Normal file
View File

@@ -0,0 +1,27 @@
//
// File: cwtfreqlimits.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef CWTFREQLIMITS_H
#define CWTFREQLIMITS_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern double getFreqFromCutoffMorse(double cutoff, double cf, double ga, double
be);
#endif
//
// File trailer for cwtfreqlimits.h
//
// [EOF]
//

69
Classes/fft.cpp Normal file
View File

@@ -0,0 +1,69 @@
//
// File: fft.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "fft.h"
#include "cwt.h"
#include "fft1.h"
#include "rt_nonfinite.h"
#include <cstring>
#include <string.h>
// Function Definitions
//
// Arguments : const double x_data[]
// const int x_size[2]
// creal_T y_data[]
// int y_size[2]
// Return Type : void
//
void fft(const double x_data[], const int x_size[2], creal_T y_data[], int
y_size[2])
{
boolean_T useRadix2;
int N2blue;
int nRows;
static double costab_data[7683];
int costab_size[2];
static double sintab_data[7683];
int sintab_size[2];
double sintabinv_data[7683];
int sintabinv_size[2];
int x[1];
creal_T b_y_data[3841];
int b_y_size[1];
if (x_size[1] != 0) {
useRadix2 = ((x_size[1] & (x_size[1] - 1)) == 0);
get_algo_sizes(x_size[1], useRadix2, &N2blue, &nRows);
generate_twiddle_tables(nRows, useRadix2, costab_data, costab_size,
sintab_data, sintab_size, sintabinv_data, sintabinv_size);
if (useRadix2) {
x[0] = x_size[1];
r2br_r2dit_trig(x_data, x, x_size[1], costab_data, sintab_data, b_y_data,
b_y_size);
} else {
x[0] = x_size[1];
dobluesteinfft(x_data, x, N2blue, x_size[1], costab_data, costab_size,
sintab_data, sintab_size, sintabinv_data, sintabinv_size,
b_y_data, b_y_size);
}
}
y_size[0] = 1;
y_size[1] = x_size[1];
N2blue = x_size[1];
if (0 <= N2blue - 1) {
std::memcpy(&y_data[0], &b_y_data[0], N2blue * sizeof(creal_T));
}
}
//
// File trailer for fft.cpp
//
// [EOF]
//

27
Classes/fft.h Normal file
View File

@@ -0,0 +1,27 @@
//
// File: fft.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef FFT_H
#define FFT_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern void fft(const double x_data[], const int x_size[2], creal_T y_data[],
int y_size[2]);
#endif
//
// File trailer for fft.h
//
// [EOF]
//

1073
Classes/fft1.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

41
Classes/fft1.h Normal file
View File

@@ -0,0 +1,41 @@
//
// File: fft1.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef FFT1_H
#define FFT1_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern void b_dobluesteinfft(const emxArray_creal_T *x, int N2, int n1, const
emxArray_real_T *costab, const emxArray_real_T *sintab, const emxArray_real_T *
sintabinv, emxArray_creal_T *y);
extern void b_r2br_r2dit_trig(const emxArray_creal_T *x, int n1_unsigned, const
emxArray_real_T *costab, const emxArray_real_T *sintab, emxArray_creal_T *y);
extern void dobluesteinfft(const double x_data[], const int x_size[1], int N2,
int n1, const double costab_data[], const int costab_size[2], const double
sintab_data[], const int sintab_size[2], const double sintabinv_data[], const
int sintabinv_size[2], creal_T y_data[], int y_size[1]);
extern void generate_twiddle_tables(int nRows, boolean_T useRadix2, double
costab_data[], int costab_size[2], double sintab_data[], int sintab_size[2],
double sintabinv_data[], int sintabinv_size[2]);
extern void get_algo_sizes(int n1, boolean_T useRadix2, int *N2blue, int *nRows);
extern void r2br_r2dit_trig(const double x_data[], const int x_size[1], int
n1_unsigned, const double costab_data[], const double sintab_data[], creal_T
y_data[], int y_size[1]);
#endif
//
// File trailer for fft1.h
//
// [EOF]
//

31
Classes/fstools.cpp Normal file
View File

@@ -0,0 +1,31 @@
#include "fstools.h"
#include <QtCore/qdir.h>
namespace fstools{
bool delDir(const QString &path) {
if (path.isEmpty()){
return false;
}
QDir dir(path);
if(!dir.exists()){
return true;
}
dir.setFilter(QDir::AllEntries | QDir::NoDotAndDotDot);
QFileInfoList fileList = dir.entryInfoList();
foreach (QFileInfo file, fileList){
if (file.isFile()) {
file.dir().remove(file.fileName());
}
else {
delDir(file.absoluteFilePath());
}
}
return dir.rmpath(dir.absolutePath());
}
bool mkDir(const QString &path, const QString &dirName) {
QDir dir(path);
dir.cd(path);
bool ok = dir.mkdir(dirName);
return ok;
}
}

13
Classes/fstools.h Normal file
View File

@@ -0,0 +1,13 @@
#ifndef FSTOOLS_H
#define FSTOOLS_H
class QString;
namespace fstools
{
bool delDir(const QString &path);
bool mkDir(const QString &path, const QString &dirName);
}
#endif // FSTOOLS_H

163
Classes/gammaln.cpp Normal file
View File

@@ -0,0 +1,163 @@
//
// File: gammaln.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "gammaln.h"
#include "cwt.h"
#include "rt_nonfinite.h"
#include <cmath>
#include <string.h>
// Function Definitions
//
// Arguments : double *x
// Return Type : void
//
void gammaln(double *x)
{
double t;
double r;
static const double table100[100] = { 0.0, 0.0, 0.69314718055994529,
1.791759469228055, 3.1780538303479458, 4.7874917427820458,
6.5792512120101012, 8.5251613610654147, 10.604602902745251,
12.801827480081469, 15.104412573075516, 17.502307845873887,
19.987214495661885, 22.552163853123425, 25.19122118273868, 27.89927138384089,
30.671860106080672, 33.505073450136891, 36.395445208033053,
39.339884187199495, 42.335616460753485, 45.380138898476908,
48.471181351835227, 51.606675567764377, 54.784729398112319,
58.003605222980518, 61.261701761002, 64.557538627006338, 67.88974313718154,
71.257038967168015, 74.658236348830158, 78.0922235533153, 81.557959456115043,
85.054467017581516, 88.580827542197682, 92.1361756036871, 95.7196945421432,
99.330612454787428, 102.96819861451381, 106.63176026064346,
110.32063971475739, 114.03421178146171, 117.77188139974507,
121.53308151543864, 125.3172711493569, 129.12393363912722,
132.95257503561632, 136.80272263732635, 140.67392364823425,
144.5657439463449, 148.47776695177302, 152.40959258449735, 156.3608363030788,
160.3311282166309, 164.32011226319517, 168.32744544842765,
172.35279713916279, 176.39584840699735, 180.45629141754378,
184.53382886144948, 188.6281734236716, 192.7390472878449, 196.86618167289,
201.00931639928152, 205.1681994826412, 209.34258675253685,
213.53224149456327, 217.73693411395422, 221.95644181913033,
226.1905483237276, 230.43904356577696, 234.70172344281826,
238.97838956183432, 243.26884900298271, 247.57291409618688,
251.89040220972319, 256.22113555000954, 260.56494097186322,
264.92164979855278, 269.29109765101981, 273.67312428569369,
278.06757344036612, 282.4742926876304, 286.893133295427, 291.32395009427029,
295.76660135076065, 300.22094864701415, 304.68685676566872,
309.1641935801469, 313.65282994987905, 318.1526396202093, 322.66349912672615,
327.1852877037752, 331.71788719692847, 336.26118197919845, 340.815058870799,
345.37940706226686, 349.95411804077025, 354.53908551944079,
359.1342053695754 };
int i;
static const double p1[8] = { 4.9452353592967269, 201.8112620856775,
2290.8383738313464, 11319.672059033808, 28557.246356716354,
38484.962284437934, 26377.487876241954, 7225.8139797002877 };
static const double p2[8] = { 4.974607845568932, 542.4138599891071,
15506.938649783649, 184793.29044456323, 1.0882047694688288E+6,
3.33815296798703E+6, 5.1066616789273527E+6, 3.0741090548505397E+6 };
static const double q1[8] = { 67.482125503037778, 1113.3323938571993,
7738.7570569353984, 27639.870744033407, 54993.102062261576,
61611.221800660023, 36351.2759150194, 8785.5363024310136 };
static const double q2[8] = { 183.03283993705926, 7765.0493214450062,
133190.38279660742, 1.1367058213219696E+6, 5.2679641174379466E+6,
1.3467014543111017E+7, 1.7827365303532742E+7, 9.5330955918443538E+6 };
static const double p4[8] = { 14745.0216605994, 2.4268133694867045E+6,
1.2147555740450932E+8, 2.6634324496309772E+9, 2.9403789566345539E+10,
1.7026657377653989E+11, 4.926125793377431E+11, 5.6062518562239514E+11 };
static const double c[7] = { -0.001910444077728, 0.00084171387781295,
-0.00059523799130430121, 0.0007936507935003503, -0.0027777777777776816,
0.083333333333333329, 0.0057083835261 };
static const double q4[8] = { 2690.5301758708993, 639388.56543000927,
4.1355999302413881E+7, 1.120872109616148E+9, 1.4886137286788137E+10,
1.0168035862724382E+11, 3.4174763455073773E+11, 4.4631581874197131E+11 };
if ((!rtIsNaN(*x)) && (!(*x < 0.0))) {
if (*x > 2.55E+305) {
*x = rtInf;
} else if (*x <= 2.2204460492503131E-16) {
*x = -std::log(*x);
} else if (*x <= 0.5) {
t = 1.0;
r = 0.0;
for (i = 0; i < 8; i++) {
r = r * *x + p1[i];
t = t * *x + q1[i];
}
*x = -std::log(*x) + *x * (*x * (r / t) + -0.57721566490153287);
} else if (*x <= 0.6796875) {
t = 1.0;
r = 0.0;
for (i = 0; i < 8; i++) {
r = r * ((*x - 0.5) - 0.5) + p2[i];
t = t * ((*x - 0.5) - 0.5) + q2[i];
}
*x = -std::log(*x) + ((*x - 0.5) - 0.5) * (((*x - 0.5) - 0.5) * (r / t) +
0.42278433509846713);
} else if ((*x == std::floor(*x)) && (*x <= 100.0)) {
*x = table100[static_cast<int>(*x) - 1];
} else if (*x <= 1.5) {
t = 1.0;
r = 0.0;
for (i = 0; i < 8; i++) {
r = r * ((*x - 0.5) - 0.5) + p1[i];
t = t * ((*x - 0.5) - 0.5) + q1[i];
}
*x = ((*x - 0.5) - 0.5) * (((*x - 0.5) - 0.5) * (r / t) +
-0.57721566490153287);
} else if (*x <= 4.0) {
t = 1.0;
r = 0.0;
for (i = 0; i < 8; i++) {
r = r * (*x - 2.0) + p2[i];
t = t * (*x - 2.0) + q2[i];
}
*x = (*x - 2.0) * ((*x - 2.0) * (r / t) + 0.42278433509846713);
} else if (*x <= 12.0) {
t = -1.0;
r = 0.0;
for (i = 0; i < 8; i++) {
r = r * (*x - 4.0) + p4[i];
t = t * (*x - 4.0) + q4[i];
}
*x = (*x - 4.0) * (r / t) + 1.791759469228055;
} else {
if (*x <= 2.25E+76) {
r = 0.0057083835261;
t = 1.0 / (*x * *x);
for (i = 0; i < 6; i++) {
r = r * t + c[i];
}
r /= *x;
} else {
r = 0.0;
}
t = std::log(*x);
*x = ((r + 0.91893853320467278) - 0.5 * t) + *x * (t - 1.0);
}
}
}
//
// File trailer for gammaln.cpp
//
// [EOF]
//

26
Classes/gammaln.h Normal file
View File

@@ -0,0 +1,26 @@
//
// File: gammaln.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef GAMMALN_H
#define GAMMALN_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern void gammaln(double *x);
#endif
//
// File trailer for gammaln.h
//
// [EOF]
//

936
Classes/hid.Win.c Normal file
View File

@@ -0,0 +1,936 @@
/*******************************************************
HIDAPI - Multi-Platform library for
communication with HID devices.
Alan Ott
Signal 11 Software
8/22/2009
Copyright 2009, All Rights Reserved.
At the discretion of the user of this library,
this software may be licensed under the terms of the
GNU General Public License v3, a BSD-Style license, or the
original HIDAPI license as outlined in the LICENSE.txt,
LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt
files located at the root of the source distribution.
These files may also be found in the public source
code repository located at:
http://github.com/signal11/hidapi .
********************************************************/
#include <windows.h>
#ifndef _NTDEF_
typedef LONG NTSTATUS;
#endif
#ifdef __MINGW32__
#include <ntdef.h>
#include <winbase.h>
#endif
#ifdef __CYGWIN__
#include <ntdef.h>
#define _wcsdup wcsdup
#endif
/*#define HIDAPI_USE_DDK*/
#ifdef __cplusplus
extern "C" {
#endif
#include <setupapi.h>
#include <winioctl.h>
#ifdef HIDAPI_USE_DDK
#include <hidsdi.h>
#endif
/* Copied from inc/ddk/hidclass.h, part of the Windows DDK. */
#define HID_OUT_CTL_CODE(id) \
CTL_CODE(FILE_DEVICE_KEYBOARD, (id), METHOD_OUT_DIRECT, FILE_ANY_ACCESS)
#define IOCTL_HID_GET_FEATURE HID_OUT_CTL_CODE(100)
#ifdef __cplusplus
} /* extern "C" */
#endif
#include <stdio.h>
#include <stdlib.h>
#include "hidapi.h"
#ifdef _MSC_VER
/* Thanks Microsoft, but I know how to use strncpy(). */
#pragma warning(disable:4996)
#endif
#ifdef __cplusplus
extern "C" {
#endif
#ifndef HIDAPI_USE_DDK
/* Since we're not building with the DDK, and the HID header
files aren't part of the SDK, we have to define all this
stuff here. In lookup_functions(), the function pointers
defined below are set. */
typedef struct _HIDD_ATTRIBUTES{
ULONG Size;
USHORT VendorID;
USHORT ProductID;
USHORT VersionNumber;
} HIDD_ATTRIBUTES, *PHIDD_ATTRIBUTES;
typedef USHORT USAGE;
typedef struct _HIDP_CAPS {
USAGE Usage;
USAGE UsagePage;
USHORT InputReportByteLength;
USHORT OutputReportByteLength;
USHORT FeatureReportByteLength;
USHORT Reserved[17];
USHORT fields_not_used_by_hidapi[10];
} HIDP_CAPS, *PHIDP_CAPS;
typedef void* PHIDP_PREPARSED_DATA;
#define HIDP_STATUS_SUCCESS 0x110000
typedef BOOLEAN (__stdcall *HidD_GetAttributes_)(HANDLE device, PHIDD_ATTRIBUTES attrib);
typedef BOOLEAN (__stdcall *HidD_GetSerialNumberString_)(HANDLE device, PVOID buffer, ULONG buffer_len);
typedef BOOLEAN (__stdcall *HidD_GetManufacturerString_)(HANDLE handle, PVOID buffer, ULONG buffer_len);
typedef BOOLEAN (__stdcall *HidD_GetProductString_)(HANDLE handle, PVOID buffer, ULONG buffer_len);
typedef BOOLEAN (__stdcall *HidD_SetFeature_)(HANDLE handle, PVOID data, ULONG length);
typedef BOOLEAN (__stdcall *HidD_GetFeature_)(HANDLE handle, PVOID data, ULONG length);
typedef BOOLEAN (__stdcall *HidD_GetIndexedString_)(HANDLE handle, ULONG string_index, PVOID buffer, ULONG buffer_len);
typedef BOOLEAN (__stdcall *HidD_GetPreparsedData_)(HANDLE handle, PHIDP_PREPARSED_DATA *preparsed_data);
typedef BOOLEAN (__stdcall *HidD_FreePreparsedData_)(PHIDP_PREPARSED_DATA preparsed_data);
typedef NTSTATUS (__stdcall *HidP_GetCaps_)(PHIDP_PREPARSED_DATA preparsed_data, HIDP_CAPS *caps);
typedef BOOLEAN (__stdcall *HidD_SetNumInputBuffers_)(HANDLE handle, ULONG number_buffers);
static HidD_GetAttributes_ HidD_GetAttributes;
static HidD_GetSerialNumberString_ HidD_GetSerialNumberString;
static HidD_GetManufacturerString_ HidD_GetManufacturerString;
static HidD_GetProductString_ HidD_GetProductString;
static HidD_SetFeature_ HidD_SetFeature;
static HidD_GetFeature_ HidD_GetFeature;
static HidD_GetIndexedString_ HidD_GetIndexedString;
static HidD_GetPreparsedData_ HidD_GetPreparsedData;
static HidD_FreePreparsedData_ HidD_FreePreparsedData;
static HidP_GetCaps_ HidP_GetCaps;
static HidD_SetNumInputBuffers_ HidD_SetNumInputBuffers;
static HMODULE lib_handle = NULL;
static BOOLEAN initialized = FALSE;
#endif /* HIDAPI_USE_DDK */
struct hid_device_ {
HANDLE device_handle;
BOOL blocking;
USHORT output_report_length;
size_t input_report_length;
void *last_error_str;
DWORD last_error_num;
BOOL read_pending;
char *read_buf;
OVERLAPPED ol;
};
static hid_device *new_hid_device()
{
hid_device *dev = (hid_device*) calloc(1, sizeof(hid_device));
dev->device_handle = INVALID_HANDLE_VALUE;
dev->blocking = TRUE;
dev->output_report_length = 0;
dev->input_report_length = 0;
dev->last_error_str = NULL;
dev->last_error_num = 0;
dev->read_pending = FALSE;
dev->read_buf = NULL;
memset(&dev->ol, 0, sizeof(dev->ol));
dev->ol.hEvent = CreateEvent(NULL, FALSE, FALSE /*inital state f=nonsignaled*/, NULL);
return dev;
}
static void free_hid_device(hid_device *dev)
{
CloseHandle(dev->ol.hEvent);
CloseHandle(dev->device_handle);
LocalFree(dev->last_error_str);
free(dev->read_buf);
free(dev);
}
static void register_error(hid_device *device, const char *op)
{
WCHAR *ptr, *msg;
FormatMessageW(FORMAT_MESSAGE_ALLOCATE_BUFFER |
FORMAT_MESSAGE_FROM_SYSTEM |
FORMAT_MESSAGE_IGNORE_INSERTS,
NULL,
GetLastError(),
MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
(LPVOID)&msg, 0/*sz*/,
NULL);
/* Get rid of the CR and LF that FormatMessage() sticks at the
end of the message. Thanks Microsoft! */
ptr = msg;
while (*ptr) {
if (*ptr == '\r') {
*ptr = 0x0000;
break;
}
ptr++;
}
/* Store the message off in the Device entry so that
the hid_error() function can pick it up. */
LocalFree(device->last_error_str);
device->last_error_str = msg;
}
#ifndef HIDAPI_USE_DDK
static int lookup_functions()
{
lib_handle = LoadLibraryA("hid.dll");
if (lib_handle) {
#define RESOLVE(x) x = (x##_)GetProcAddress(lib_handle, #x); if (!x) return -1;
RESOLVE(HidD_GetAttributes);
RESOLVE(HidD_GetSerialNumberString);
RESOLVE(HidD_GetManufacturerString);
RESOLVE(HidD_GetProductString);
RESOLVE(HidD_SetFeature);
RESOLVE(HidD_GetFeature);
RESOLVE(HidD_GetIndexedString);
RESOLVE(HidD_GetPreparsedData);
RESOLVE(HidD_FreePreparsedData);
RESOLVE(HidP_GetCaps);
RESOLVE(HidD_SetNumInputBuffers);
#undef RESOLVE
}
else
return -1;
return 0;
}
#endif
static HANDLE open_device(const char *path, BOOL enumerate)
{
HANDLE handle;
DWORD desired_access = (enumerate)? 0: (GENERIC_WRITE | GENERIC_READ);
DWORD share_mode = (enumerate)?
FILE_SHARE_READ|FILE_SHARE_WRITE:
FILE_SHARE_READ;
handle = CreateFileA(path,
desired_access,
share_mode,
NULL,
OPEN_EXISTING,
FILE_FLAG_OVERLAPPED,/*FILE_ATTRIBUTE_NORMAL,*/
0);
return handle;
}
int HID_API_EXPORT hid_init(void)
{
#ifndef HIDAPI_USE_DDK
if (!initialized) {
if (lookup_functions() < 0) {
hid_exit();
return -1;
}
initialized = TRUE;
}
#endif
return 0;
}
int HID_API_EXPORT hid_exit(void)
{
#ifndef HIDAPI_USE_DDK
if (lib_handle)
FreeLibrary(lib_handle);
lib_handle = NULL;
initialized = FALSE;
#endif
return 0;
}
struct hid_device_info HID_API_EXPORT * HID_API_CALL hid_enumerate(unsigned short vendor_id, unsigned short product_id)
{
BOOL res;
struct hid_device_info *root = NULL; /* return object */
struct hid_device_info *cur_dev = NULL;
/* Windows objects for interacting with the driver. */
GUID InterfaceClassGuid = {0x4d1e55b2, 0xf16f, 0x11cf, {0x88, 0xcb, 0x00, 0x11, 0x11, 0x00, 0x00, 0x30} };
SP_DEVINFO_DATA devinfo_data;
SP_DEVICE_INTERFACE_DATA device_interface_data;
SP_DEVICE_INTERFACE_DETAIL_DATA_A *device_interface_detail_data = NULL;
HDEVINFO device_info_set = INVALID_HANDLE_VALUE;
int device_index = 0;
int i;
if (hid_init() < 0)
return NULL;
/* Initialize the Windows objects. */
memset(&devinfo_data, 0x0, sizeof(devinfo_data));
devinfo_data.cbSize = sizeof(SP_DEVINFO_DATA);
device_interface_data.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA);
/* Get information for all the devices belonging to the HID class. */
device_info_set = SetupDiGetClassDevsA(&InterfaceClassGuid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE);
/* Iterate over each device in the HID class, looking for the right one. */
for (;;) {
HANDLE write_handle = INVALID_HANDLE_VALUE;
DWORD required_size = 0;
HIDD_ATTRIBUTES attrib;
res = SetupDiEnumDeviceInterfaces(device_info_set,
NULL,
&InterfaceClassGuid,
device_index,
&device_interface_data);
if (!res) {
/* A return of FALSE from this function means that
there are no more devices. */
break;
}
/* Call with 0-sized detail size, and let the function
tell us how long the detail struct needs to be. The
size is put in &required_size. */
res = SetupDiGetDeviceInterfaceDetailA(device_info_set,
&device_interface_data,
NULL,
0,
&required_size,
NULL);
/* Allocate a long enough structure for device_interface_detail_data. */
device_interface_detail_data = (SP_DEVICE_INTERFACE_DETAIL_DATA_A*) malloc(required_size);
device_interface_detail_data->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A);
/* Get the detailed data for this device. The detail data gives us
the device path for this device, which is then passed into
CreateFile() to get a handle to the device. */
res = SetupDiGetDeviceInterfaceDetailA(device_info_set,
&device_interface_data,
device_interface_detail_data,
required_size,
NULL,
NULL);
if (!res) {
/* register_error(dev, "Unable to call SetupDiGetDeviceInterfaceDetail");
Continue to the next device. */
goto cont;
}
/* Make sure this device is of Setup Class "HIDClass" and has a
driver bound to it. */
for (i = 0; ; i++) {
char driver_name[256];
/* Populate devinfo_data. This function will return failure
when there are no more interfaces left. */
res = SetupDiEnumDeviceInfo(device_info_set, i, &devinfo_data);
if (!res)
goto cont;
res = SetupDiGetDeviceRegistryPropertyA(device_info_set, &devinfo_data,
SPDRP_CLASS, NULL, (PBYTE)driver_name, sizeof(driver_name), NULL);
if (!res)
goto cont;
if (strcmp(driver_name, "HIDClass") == 0) {
/* See if there's a driver bound. */
res = SetupDiGetDeviceRegistryPropertyA(device_info_set, &devinfo_data,
SPDRP_DRIVER, NULL, (PBYTE)driver_name, sizeof(driver_name), NULL);
if (res)
break;
}
}
//wprintf(L"HandleName: %s\n", device_interface_detail_data->DevicePath);
/* Open a handle to the device */
write_handle = open_device(device_interface_detail_data->DevicePath, TRUE);
/* Check validity of write_handle. */
if (write_handle == INVALID_HANDLE_VALUE) {
/* Unable to open the device. */
//register_error(dev, "CreateFile");
goto cont_close;
}
/* Get the Vendor ID and Product ID for this device. */
attrib.Size = sizeof(HIDD_ATTRIBUTES);
HidD_GetAttributes(write_handle, &attrib);
//wprintf(L"Product/Vendor: %x %x\n", attrib.ProductID, attrib.VendorID);
/* Check the VID/PID to see if we should add this
device to the enumeration list. */
if ((vendor_id == 0x0 || attrib.VendorID == vendor_id) &&
(product_id == 0x0 || attrib.ProductID == product_id)) {
#define WSTR_LEN 512
const char *str;
struct hid_device_info *tmp;
PHIDP_PREPARSED_DATA pp_data = NULL;
HIDP_CAPS caps;
BOOLEAN res;
NTSTATUS nt_res;
wchar_t wstr[WSTR_LEN]; /* TODO: Determine Size */
size_t len;
/* VID/PID match. Create the record. */
tmp = (struct hid_device_info*) calloc(1, sizeof(struct hid_device_info));
if (cur_dev) {
cur_dev->next = tmp;
}
else {
root = tmp;
}
cur_dev = tmp;
/* Get the Usage Page and Usage for this device. */
res = HidD_GetPreparsedData(write_handle, &pp_data);
if (res) {
nt_res = HidP_GetCaps(pp_data, &caps);
if (nt_res == HIDP_STATUS_SUCCESS) {
cur_dev->usage_page = caps.UsagePage;
cur_dev->usage = caps.Usage;
}
HidD_FreePreparsedData(pp_data);
}
/* Fill out the record */
cur_dev->next = NULL;
str = device_interface_detail_data->DevicePath;
if (str) {
len = strlen(str);
cur_dev->path = (char*) calloc(len+1, sizeof(char));
strncpy(cur_dev->path, str, len+1);
cur_dev->path[len] = '\0';
}
else
cur_dev->path = NULL;
/* Serial Number */
res = HidD_GetSerialNumberString(write_handle, wstr, sizeof(wstr));
wstr[WSTR_LEN-1] = 0x0000;
if (res) {
cur_dev->serial_number = _wcsdup(wstr);
}
/* Manufacturer String */
res = HidD_GetManufacturerString(write_handle, wstr, sizeof(wstr));
wstr[WSTR_LEN-1] = 0x0000;
if (res) {
cur_dev->manufacturer_string = _wcsdup(wstr);
}
/* Product String */
res = HidD_GetProductString(write_handle, wstr, sizeof(wstr));
wstr[WSTR_LEN-1] = 0x0000;
if (res) {
cur_dev->product_string = _wcsdup(wstr);
}
/* VID/PID */
cur_dev->vendor_id = attrib.VendorID;
cur_dev->product_id = attrib.ProductID;
/* Release Number */
cur_dev->release_number = attrib.VersionNumber;
/* Interface Number. It can sometimes be parsed out of the path
on Windows if a device has multiple interfaces. See
http://msdn.microsoft.com/en-us/windows/hardware/gg487473 or
search for "Hardware IDs for HID Devices" at MSDN. If it's not
in the path, it's set to -1. */
cur_dev->interface_number = -1;
if (cur_dev->path) {
char *interface_component = strstr(cur_dev->path, "&mi_");
if (interface_component) {
char *hex_str = interface_component + 4;
char *endptr = NULL;
cur_dev->interface_number = strtol(hex_str, &endptr, 16);
if (endptr == hex_str) {
/* The parsing failed. Set interface_number to -1. */
cur_dev->interface_number = -1;
}
}
}
}
cont_close:
CloseHandle(write_handle);
cont:
/* We no longer need the detail data. It can be freed */
free(device_interface_detail_data);
device_index++;
}
/* Close the device information handle. */
SetupDiDestroyDeviceInfoList(device_info_set);
return root;
}
void HID_API_EXPORT HID_API_CALL hid_free_enumeration(struct hid_device_info *devs)
{
/* TODO: Merge this with the Linux version. This function is platform-independent. */
struct hid_device_info *d = devs;
while (d) {
struct hid_device_info *next = d->next;
free(d->path);
free(d->serial_number);
free(d->manufacturer_string);
free(d->product_string);
free(d);
d = next;
}
}
HID_API_EXPORT hid_device * HID_API_CALL hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number)
{
/* TODO: Merge this functions with the Linux version. This function should be platform independent. */
struct hid_device_info *devs, *cur_dev;
const char *path_to_open = NULL;
hid_device *handle = NULL;
devs = hid_enumerate(vendor_id, product_id);
cur_dev = devs;
while (cur_dev) {
if (cur_dev->vendor_id == vendor_id &&
cur_dev->product_id == product_id) {
if (serial_number) {
if (wcscmp(serial_number, cur_dev->serial_number) == 0) {
path_to_open = cur_dev->path;
break;
}
}
else {
path_to_open = cur_dev->path;
break;
}
}
cur_dev = cur_dev->next;
}
if (path_to_open) {
/* Open the device */
handle = hid_open_path(path_to_open);
}
hid_free_enumeration(devs);
return handle;
}
HID_API_EXPORT hid_device * HID_API_CALL hid_open_path(const char *path)
{
hid_device *dev;
HIDP_CAPS caps;
PHIDP_PREPARSED_DATA pp_data = NULL;
BOOLEAN res;
NTSTATUS nt_res;
if (hid_init() < 0) {
return NULL;
}
dev = new_hid_device();
/* Open a handle to the device */
dev->device_handle = open_device(path, FALSE);
/* Check validity of write_handle. */
if (dev->device_handle == INVALID_HANDLE_VALUE) {
/* Unable to open the device. */
register_error(dev, "CreateFile");
goto err;
}
/* Set the Input Report buffer size to 64 reports. */
res = HidD_SetNumInputBuffers(dev->device_handle, 64);
if (!res) {
register_error(dev, "HidD_SetNumInputBuffers");
goto err;
}
/* Get the Input Report length for the device. */
res = HidD_GetPreparsedData(dev->device_handle, &pp_data);
if (!res) {
register_error(dev, "HidD_GetPreparsedData");
goto err;
}
nt_res = HidP_GetCaps(pp_data, &caps);
if (nt_res != HIDP_STATUS_SUCCESS) {
register_error(dev, "HidP_GetCaps");
goto err_pp_data;
}
dev->output_report_length = caps.OutputReportByteLength;
dev->input_report_length = caps.InputReportByteLength;
HidD_FreePreparsedData(pp_data);
dev->read_buf = (char*) malloc(dev->input_report_length);
return dev;
err_pp_data:
HidD_FreePreparsedData(pp_data);
err:
free_hid_device(dev);
return NULL;
}
int HID_API_EXPORT HID_API_CALL hid_write(hid_device *dev, const unsigned char *data, size_t length)
{
DWORD bytes_written;
BOOL res;
OVERLAPPED ol;
unsigned char *buf;
memset(&ol, 0, sizeof(ol));
/* Make sure the right number of bytes are passed to WriteFile. Windows
expects the number of bytes which are in the _longest_ report (plus
one for the report number) bytes even if the data is a report
which is shorter than that. Windows gives us this value in
caps.OutputReportByteLength. If a user passes in fewer bytes than this,
create a temporary buffer which is the proper size. */
if (length >= dev->output_report_length) {
/* The user passed the right number of bytes. Use the buffer as-is. */
buf = (unsigned char *) data;
} else {
/* Create a temporary buffer and copy the user's data
into it, padding the rest with zeros. */
buf = (unsigned char *) malloc(dev->output_report_length);
memcpy(buf, data, length);
memset(buf + length, 0, dev->output_report_length - length);
length = dev->output_report_length;
}
res = WriteFile(dev->device_handle, buf, length, NULL, &ol);
if (!res) {
if (GetLastError() != ERROR_IO_PENDING) {
/* WriteFile() failed. Return error. */
register_error(dev, "WriteFile");
bytes_written = -1;
goto end_of_function;
}
}
/* Wait here until the write is done. This makes
hid_write() synchronous. */
res = GetOverlappedResult(dev->device_handle, &ol, &bytes_written, TRUE/*wait*/);
if (!res) {
/* The Write operation failed. */
register_error(dev, "WriteFile");
bytes_written = -1;
goto end_of_function;
}
end_of_function:
if (buf != data)
free(buf);
return bytes_written;
}
int HID_API_EXPORT HID_API_CALL hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds)
{
DWORD bytes_read = 0;
size_t copy_len = 0;
BOOL res;
/* Copy the handle for convenience. */
HANDLE ev = dev->ol.hEvent;
if (!dev->read_pending) {
/* Start an Overlapped I/O read. */
dev->read_pending = TRUE;
memset(dev->read_buf, 0, dev->input_report_length);
ResetEvent(ev);
res = ReadFile(dev->device_handle, dev->read_buf, dev->input_report_length, &bytes_read, &dev->ol);
if (!res) {
if (GetLastError() != ERROR_IO_PENDING) {
/* ReadFile() has failed.
Clean up and return error. */
CancelIo(dev->device_handle);
dev->read_pending = FALSE;
goto end_of_function;
}
}
}
if (milliseconds >= 0) {
/* See if there is any data yet. */
res = WaitForSingleObject(ev, milliseconds);
if (res != WAIT_OBJECT_0) {
/* There was no data this time. Return zero bytes available,
but leave the Overlapped I/O running. */
return 0;
}
}
/* Either WaitForSingleObject() told us that ReadFile has completed, or
we are in non-blocking mode. Get the number of bytes read. The actual
data has been copied to the data[] array which was passed to ReadFile(). */
res = GetOverlappedResult(dev->device_handle, &dev->ol, &bytes_read, TRUE/*wait*/);
/* Set pending back to false, even if GetOverlappedResult() returned error. */
dev->read_pending = FALSE;
if (res && bytes_read > 0) {
if (dev->read_buf[0] == 0x0) {
/* If report numbers aren't being used, but Windows sticks a report
number (0x0) on the beginning of the report anyway. To make this
work like the other platforms, and to make it work more like the
HID spec, we'll skip over this byte. */
bytes_read--;
copy_len = length > bytes_read ? bytes_read : length;
memcpy(data, dev->read_buf+1, copy_len);
}
else {
/* Copy the whole buffer, report number and all. */
copy_len = length > bytes_read ? bytes_read : length;
memcpy(data, dev->read_buf, copy_len);
}
}
end_of_function:
if (!res) {
register_error(dev, "GetOverlappedResult");
return -1;
}
return copy_len;
}
int HID_API_EXPORT HID_API_CALL hid_read(hid_device *dev, unsigned char *data, size_t length)
{
return hid_read_timeout(dev, data, length, (dev->blocking)? -1: 0);
}
int HID_API_EXPORT HID_API_CALL hid_set_nonblocking(hid_device *dev, int nonblock)
{
dev->blocking = !nonblock;
return 0; /* Success */
}
int HID_API_EXPORT HID_API_CALL hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length)
{
BOOL res = HidD_SetFeature(dev->device_handle, (PVOID)data, length);
if (!res) {
register_error(dev, "HidD_SetFeature");
return -1;
}
return length;
}
int HID_API_EXPORT HID_API_CALL hid_get_feature_report(hid_device *dev, unsigned char *data, size_t length)
{
BOOL res;
#if 0
res = HidD_GetFeature(dev->device_handle, data, length);
if (!res) {
register_error(dev, "HidD_GetFeature");
return -1;
}
return 0; /* HidD_GetFeature() doesn't give us an actual length, unfortunately */
#else
DWORD bytes_returned;
OVERLAPPED ol;
memset(&ol, 0, sizeof(ol));
res = DeviceIoControl(dev->device_handle,
IOCTL_HID_GET_FEATURE,
data, length,
data, length,
&bytes_returned, &ol);
if (!res) {
if (GetLastError() != ERROR_IO_PENDING) {
/* DeviceIoControl() failed. Return error. */
register_error(dev, "Send Feature Report DeviceIoControl");
return -1;
}
}
/* Wait here until the write is done. This makes
hid_get_feature_report() synchronous. */
res = GetOverlappedResult(dev->device_handle, &ol, &bytes_returned, TRUE/*wait*/);
if (!res) {
/* The operation failed. */
register_error(dev, "Send Feature Report GetOverLappedResult");
return -1;
}
/* bytes_returned does not include the first byte which contains the
report ID. The data buffer actually contains one more byte than
bytes_returned. */
bytes_returned++;
return bytes_returned;
#endif
}
void HID_API_EXPORT HID_API_CALL hid_close(hid_device *dev)
{
if (!dev)
return;
CancelIo(dev->device_handle);
free_hid_device(dev);
}
int HID_API_EXPORT_CALL HID_API_CALL hid_get_manufacturer_string(hid_device *dev, wchar_t *string, size_t maxlen)
{
BOOL res;
res = HidD_GetManufacturerString(dev->device_handle, string, sizeof(wchar_t) * maxlen);
if (!res) {
register_error(dev, "HidD_GetManufacturerString");
return -1;
}
return 0;
}
int HID_API_EXPORT_CALL HID_API_CALL hid_get_product_string(hid_device *dev, wchar_t *string, size_t maxlen)
{
BOOL res;
res = HidD_GetProductString(dev->device_handle, string, sizeof(wchar_t) * maxlen);
if (!res) {
register_error(dev, "HidD_GetProductString");
return -1;
}
return 0;
}
int HID_API_EXPORT_CALL HID_API_CALL hid_get_serial_number_string(hid_device *dev, wchar_t *string, size_t maxlen)
{
BOOL res;
res = HidD_GetSerialNumberString(dev->device_handle, string, sizeof(wchar_t) * maxlen);
if (!res) {
register_error(dev, "HidD_GetSerialNumberString");
return -1;
}
return 0;
}
int HID_API_EXPORT_CALL HID_API_CALL hid_get_indexed_string(hid_device *dev, int string_index, wchar_t *string, size_t maxlen)
{
BOOL res;
res = HidD_GetIndexedString(dev->device_handle, string_index, string, sizeof(wchar_t) * maxlen);
if (!res) {
register_error(dev, "HidD_GetIndexedString");
return -1;
}
return 0;
}
HID_API_EXPORT const wchar_t * HID_API_CALL hid_error(hid_device *dev)
{
return (wchar_t*)dev->last_error_str;
}
/*#define PICPGM*/
/*#define S11*/
#define P32
#ifdef S11
unsigned short VendorID = 0xa0a0;
unsigned short ProductID = 0x0001;
#endif
#ifdef P32
unsigned short VendorID = 0x04d8;
unsigned short ProductID = 0x3f;
#endif
#ifdef PICPGM
unsigned short VendorID = 0x04d8;
unsigned short ProductID = 0x0033;
#endif
#if 0
int __cdecl main(int argc, char* argv[])
{
int res;
unsigned char buf[65];
UNREFERENCED_PARAMETER(argc);
UNREFERENCED_PARAMETER(argv);
/* Set up the command buffer. */
memset(buf,0x00,sizeof(buf));
buf[0] = 0;
buf[1] = 0x81;
/* Open the device. */
int handle = open(VendorID, ProductID, L"12345");
if (handle < 0)
printf("unable to open device\n");
/* Toggle LED (cmd 0x80) */
buf[1] = 0x80;
res = write(handle, buf, 65);
if (res < 0)
printf("Unable to write()\n");
/* Request state (cmd 0x81) */
buf[1] = 0x81;
write(handle, buf, 65);
if (res < 0)
printf("Unable to write() (2)\n");
/* Read requested state */
read(handle, buf, 65);
if (res < 0)
printf("Unable to read()\n");
/* Print out the returned buffer. */
for (int i = 0; i < 4; i++)
printf("buf[%d]: %d\n", i, buf[i]);
return 0;
}
#endif
#ifdef __cplusplus
} /* extern "C" */
#endif

385
Classes/hidapi.h Normal file
View File

@@ -0,0 +1,385 @@
/*******************************************************
HIDAPI - Multi-Platform library for
communication with HID devices.
Alan Ott
Signal 11 Software
8/22/2009
Copyright 2009, All Rights Reserved.
At the discretion of the user of this library,
this software may be licensed under the terms of the
GNU Public License v3, a BSD-Style license, or the
original HIDAPI license as outlined in the LICENSE.txt,
LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt
files located at the root of the source distribution.
These files may also be found in the public source
code repository located at:
http://github.com/signal11/hidapi .
********************************************************/
/** @file
* @defgroup API hidapi API
*/
#ifndef HIDAPI_H__
#define HIDAPI_H__
#include <wchar.h>
#ifdef _WIN32
#define HID_API_EXPORT __declspec(dllexport)
#define HID_API_CALL
#else
#define HID_API_EXPORT /**< API export macro */
#define HID_API_CALL /**< API call macro */
#endif
#define HID_API_EXPORT_CALL HID_API_EXPORT HID_API_CALL /**< API export and call macro*/
#ifdef __cplusplus
extern "C" {
#endif
struct hid_device_;
typedef struct hid_device_ hid_device; /**< opaque hidapi structure */
/** hidapi info structure */
struct hid_device_info {
/** Platform-specific device path */
char *path;
/** Device Vendor ID */
unsigned short vendor_id;
/** Device Product ID */
unsigned short product_id;
/** Serial Number */
wchar_t *serial_number;
/** Device Release Number in binary-coded decimal,
also known as Device Version Number */
unsigned short release_number;
/** Manufacturer String */
wchar_t *manufacturer_string;
/** Product string */
wchar_t *product_string;
/** Usage Page for this Device/Interface
(Windows/Mac only). */
unsigned short usage_page;
/** Usage for this Device/Interface
(Windows/Mac only).*/
unsigned short usage;
/** The USB interface which this logical device
represents. Valid on both Linux implementations
in all cases, and valid on the Windows implementation
only if the device contains more than one interface. */
int interface_number;
/** Pointer to the next device */
struct hid_device_info *next;
};
/** @brief Initialize the HIDAPI library.
This function initializes the HIDAPI library. Calling it is not
strictly necessary, as it will be called automatically by
hid_enumerate() and any of the hid_open_*() functions if it is
needed. This function should be called at the beginning of
execution however, if there is a chance of HIDAPI handles
being opened by different threads simultaneously.
@ingroup API
@returns
This function returns 0 on success and -1 on error.
*/
int HID_API_EXPORT HID_API_CALL hid_init(void);
/** @brief Finalize the HIDAPI library.
This function frees all of the static data associated with
HIDAPI. It should be called at the end of execution to avoid
memory leaks.
@ingroup API
@returns
This function returns 0 on success and -1 on error.
*/
int HID_API_EXPORT HID_API_CALL hid_exit(void);
/** @brief Enumerate the HID Devices.
This function returns a linked list of all the HID devices
attached to the system which match vendor_id and product_id.
If @p vendor_id is set to 0 then any vendor matches.
If @p product_id is set to 0 then any product matches.
If @p vendor_id and @p product_id are both set to 0, then
all HID devices will be returned.
@ingroup API
@param vendor_id The Vendor ID (VID) of the types of device
to open.
@param product_id The Product ID (PID) of the types of
device to open.
@returns
This function returns a pointer to a linked list of type
struct #hid_device, containing information about the HID devices
attached to the system, or NULL in the case of failure. Free
this linked list by calling hid_free_enumeration().
*/
struct hid_device_info HID_API_EXPORT * HID_API_CALL hid_enumerate(unsigned short vendor_id, unsigned short product_id);
/** @brief Free an enumeration Linked List
This function frees a linked list created by hid_enumerate().
@ingroup API
@param devs Pointer to a list of struct_device returned from
hid_enumerate().
*/
void HID_API_EXPORT HID_API_CALL hid_free_enumeration(struct hid_device_info *devs);
/** @brief Open a HID device using a Vendor ID (VID), Product ID
(PID) and optionally a serial number.
If @p serial_number is NULL, the first device with the
specified VID and PID is opened.
@ingroup API
@param vendor_id The Vendor ID (VID) of the device to open.
@param product_id The Product ID (PID) of the device to open.
@param serial_number The Serial Number of the device to open
(Optionally NULL).
@returns
This function returns a pointer to a #hid_device object on
success or NULL on failure.
*/
HID_API_EXPORT hid_device * HID_API_CALL hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number);
/** @brief Open a HID device by its path name.
The path name be determined by calling hid_enumerate(), or a
platform-specific path name can be used (eg: /dev/hidraw0 on
Linux).
@ingroup API
@param path The path name of the device to open
@returns
This function returns a pointer to a #hid_device object on
success or NULL on failure.
*/
HID_API_EXPORT hid_device * HID_API_CALL hid_open_path(const char *path);
/** @brief Write an Output report to a HID device.
The first byte of @p data[] must contain the Report ID. For
devices which only support a single report, this must be set
to 0x0. The remaining bytes contain the report data. Since
the Report ID is mandatory, calls to hid_write() will always
contain one more byte than the report contains. For example,
if a hid report is 16 bytes long, 17 bytes must be passed to
hid_write(), the Report ID (or 0x0, for devices with a
single report), followed by the report data (16 bytes). In
this example, the length passed in would be 17.
hid_write() will send the data on the first OUT endpoint, if
one exists. If it does not, it will send the data through
the Control Endpoint (Endpoint 0).
@ingroup API
@param device A device handle returned from hid_open().
@param data The data to send, including the report number as
the first byte.
@param length The length in bytes of the data to send.
@returns
This function returns the actual number of bytes written and
-1 on error.
*/
int HID_API_EXPORT HID_API_CALL hid_write(hid_device *device, const unsigned char *data, size_t length);
/** @brief Read an Input report from a HID device with timeout.
Input reports are returned
to the host through the INTERRUPT IN endpoint. The first byte will
contain the Report number if the device uses numbered reports.
@ingroup API
@param device A device handle returned from hid_open().
@param data A buffer to put the read data into.
@param length The number of bytes to read. For devices with
multiple reports, make sure to read an extra byte for
the report number.
@param milliseconds timeout in milliseconds or -1 for blocking wait.
@returns
This function returns the actual number of bytes read and
-1 on error.
*/
int HID_API_EXPORT HID_API_CALL hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds);
/** @brief Read an Input report from a HID device.
Input reports are returned
to the host through the INTERRUPT IN endpoint. The first byte will
contain the Report number if the device uses numbered reports.
@ingroup API
@param device A device handle returned from hid_open().
@param data A buffer to put the read data into.
@param length The number of bytes to read. For devices with
multiple reports, make sure to read an extra byte for
the report number.
@returns
This function returns the actual number of bytes read and
-1 on error.
*/
int HID_API_EXPORT HID_API_CALL hid_read(hid_device *device, unsigned char *data, size_t length);
/** @brief Set the device handle to be non-blocking.
In non-blocking mode calls to hid_read() will return
immediately with a value of 0 if there is no data to be
read. In blocking mode, hid_read() will wait (block) until
there is data to read before returning.
Nonblocking can be turned on and off at any time.
@ingroup API
@param device A device handle returned from hid_open().
@param nonblock enable or not the nonblocking reads
- 1 to enable nonblocking
- 0 to disable nonblocking.
@returns
This function returns 0 on success and -1 on error.
*/
int HID_API_EXPORT HID_API_CALL hid_set_nonblocking(hid_device *device, int nonblock);
/** @brief Send a Feature report to the device.
Feature reports are sent over the Control endpoint as a
Set_Report transfer. The first byte of @p data[] must
contain the Report ID. For devices which only support a
single report, this must be set to 0x0. The remaining bytes
contain the report data. Since the Report ID is mandatory,
calls to hid_send_feature_report() will always contain one
more byte than the report contains. For example, if a hid
report is 16 bytes long, 17 bytes must be passed to
hid_send_feature_report(): the Report ID (or 0x0, for
devices which do not use numbered reports), followed by the
report data (16 bytes). In this example, the length passed
in would be 17.
@ingroup API
@param device A device handle returned from hid_open().
@param data The data to send, including the report number as
the first byte.
@param length The length in bytes of the data to send, including
the report number.
@returns
This function returns the actual number of bytes written and
-1 on error.
*/
int HID_API_EXPORT HID_API_CALL hid_send_feature_report(hid_device *device, const unsigned char *data, size_t length);
/** @brief Get a feature report from a HID device.
Make sure to set the first byte of @p data[] to the Report
ID of the report to be read. Make sure to allow space for
this extra byte in @p data[].
@ingroup API
@param device A device handle returned from hid_open().
@param data A buffer to put the read data into, including
the Report ID. Set the first byte of @p data[] to the
Report ID of the report to be read.
@param length The number of bytes to read, including an
extra byte for the report ID. The buffer can be longer
than the actual report.
@returns
This function returns the number of bytes read and
-1 on error.
*/
int HID_API_EXPORT HID_API_CALL hid_get_feature_report(hid_device *device, unsigned char *data, size_t length);
/** @brief Close a HID device.
@ingroup API
@param device A device handle returned from hid_open().
*/
void HID_API_EXPORT HID_API_CALL hid_close(hid_device *device);
/** @brief Get The Manufacturer String from a HID device.
@ingroup API
@param device A device handle returned from hid_open().
@param string A wide string buffer to put the data into.
@param maxlen The length of the buffer in multiples of wchar_t.
@returns
This function returns 0 on success and -1 on error.
*/
int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *device, wchar_t *string, size_t maxlen);
/** @brief Get The Product String from a HID device.
@ingroup API
@param device A device handle returned from hid_open().
@param string A wide string buffer to put the data into.
@param maxlen The length of the buffer in multiples of wchar_t.
@returns
This function returns 0 on success and -1 on error.
*/
int HID_API_EXPORT_CALL hid_get_product_string(hid_device *device, wchar_t *string, size_t maxlen);
/** @brief Get The Serial Number String from a HID device.
@ingroup API
@param device A device handle returned from hid_open().
@param string A wide string buffer to put the data into.
@param maxlen The length of the buffer in multiples of wchar_t.
@returns
This function returns 0 on success and -1 on error.
*/
int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *device, wchar_t *string, size_t maxlen);
/** @brief Get a string from a HID device, based on its string index.
@ingroup API
@param device A device handle returned from hid_open().
@param string_index The index of the string to get.
@param string A wide string buffer to put the data into.
@param maxlen The length of the buffer in multiples of wchar_t.
@returns
This function returns 0 on success and -1 on error.
*/
int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *device, int string_index, wchar_t *string, size_t maxlen);
/** @brief Get a string describing the last error which occurred.
@ingroup API
@param device A device handle returned from hid_open().
@returns
This function returns a string containing the last error
which occurred or NULL if none has occurred.
*/
HID_API_EXPORT const wchar_t* HID_API_CALL hid_error(hid_device *device);
#ifdef __cplusplus
}
#endif
#endif

191
Classes/ifft.cpp Normal file
View File

@@ -0,0 +1,191 @@
//
// File: ifft.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "ifft.h"
#include "cwt.h"
#include "cwt_emxutil.h"
#include "fft1.h"
#include "rt_nonfinite.h"
#include <cmath>
#include <string.h>
// Function Definitions
//
// Arguments : const emxArray_creal_T *x
// emxArray_creal_T *y
// Return Type : void
//
void ifft(const emxArray_creal_T *x, emxArray_creal_T *y)
{
emxArray_creal_T *b;
int i;
int nd2;
emxArray_creal_T *b_y1;
int n2;
emxArray_real_T *costab1q;
int i1;
emxArray_real_T *costab;
emxArray_real_T *sintab;
emxArray_real_T *sintabinv;
boolean_T useRadix2;
int N2blue;
double e;
int n;
int k;
emxInit_creal_T(&b, 2);
i = b->size[0] * b->size[1];
b->size[0] = x->size[1];
b->size[1] = x->size[0];
emxEnsureCapacity_creal_T(b, i);
nd2 = x->size[0];
for (i = 0; i < nd2; i++) {
n2 = x->size[1];
for (i1 = 0; i1 < n2; i1++) {
b->data[i1 + b->size[0] * i] = x->data[i + x->size[0] * i1];
}
}
emxInit_creal_T(&b_y1, 2);
emxInit_real_T(&costab1q, 2);
emxInit_real_T(&costab, 2);
emxInit_real_T(&sintab, 2);
emxInit_real_T(&sintabinv, 2);
if ((b->size[0] == 0) || (b->size[1] == 0) || (x->size[1] == 0)) {
i = b_y1->size[0] * b_y1->size[1];
b_y1->size[0] = x->size[1];
b_y1->size[1] = b->size[1];
emxEnsureCapacity_creal_T(b_y1, i);
if (x->size[1] > b->size[0]) {
nd2 = b->size[1];
for (i = 0; i < nd2; i++) {
n2 = b_y1->size[0];
for (i1 = 0; i1 < n2; i1++) {
b_y1->data[i1 + b_y1->size[0] * i].re = 0.0;
b_y1->data[i1 + b_y1->size[0] * i].im = 0.0;
}
}
}
} else {
useRadix2 = ((x->size[1] & (x->size[1] - 1)) == 0);
get_algo_sizes(x->size[1], useRadix2, &N2blue, &nd2);
e = 6.2831853071795862 / static_cast<double>(nd2);
n = nd2 / 2 / 2;
i = costab1q->size[0] * costab1q->size[1];
costab1q->size[0] = 1;
costab1q->size[1] = n + 1;
emxEnsureCapacity_real_T(costab1q, i);
costab1q->data[0] = 1.0;
nd2 = n / 2 - 1;
for (k = 0; k <= nd2; k++) {
costab1q->data[k + 1] = std::cos(e * (static_cast<double>(k) + 1.0));
}
i = nd2 + 2;
i1 = n - 1;
for (k = i; k <= i1; k++) {
costab1q->data[k] = std::sin(e * static_cast<double>((n - k)));
}
costab1q->data[n] = 0.0;
if (!useRadix2) {
n = costab1q->size[1] - 1;
n2 = (costab1q->size[1] - 1) << 1;
i = costab->size[0] * costab->size[1];
costab->size[0] = 1;
costab->size[1] = n2 + 1;
emxEnsureCapacity_real_T(costab, i);
i = sintab->size[0] * sintab->size[1];
sintab->size[0] = 1;
sintab->size[1] = n2 + 1;
emxEnsureCapacity_real_T(sintab, i);
costab->data[0] = 1.0;
sintab->data[0] = 0.0;
i = sintabinv->size[0] * sintabinv->size[1];
sintabinv->size[0] = 1;
sintabinv->size[1] = n2 + 1;
emxEnsureCapacity_real_T(sintabinv, i);
for (k = 0; k < n; k++) {
sintabinv->data[k + 1] = costab1q->data[(n - k) - 1];
}
i = costab1q->size[1];
for (k = i; k <= n2; k++) {
sintabinv->data[k] = costab1q->data[k - n];
}
for (k = 0; k < n; k++) {
costab->data[k + 1] = costab1q->data[k + 1];
sintab->data[k + 1] = -costab1q->data[(n - k) - 1];
}
i = costab1q->size[1];
for (k = i; k <= n2; k++) {
costab->data[k] = -costab1q->data[n2 - k];
sintab->data[k] = -costab1q->data[k - n];
}
} else {
n = costab1q->size[1] - 1;
n2 = (costab1q->size[1] - 1) << 1;
i = costab->size[0] * costab->size[1];
costab->size[0] = 1;
costab->size[1] = n2 + 1;
emxEnsureCapacity_real_T(costab, i);
i = sintab->size[0] * sintab->size[1];
sintab->size[0] = 1;
sintab->size[1] = n2 + 1;
emxEnsureCapacity_real_T(sintab, i);
costab->data[0] = 1.0;
sintab->data[0] = 0.0;
for (k = 0; k < n; k++) {
costab->data[k + 1] = costab1q->data[k + 1];
sintab->data[k + 1] = costab1q->data[(n - k) - 1];
}
i = costab1q->size[1];
for (k = i; k <= n2; k++) {
costab->data[k] = -costab1q->data[n2 - k];
sintab->data[k] = costab1q->data[k - n];
}
sintabinv->size[0] = 1;
sintabinv->size[1] = 0;
}
if (useRadix2) {
b_r2br_r2dit_trig(b, x->size[1], costab, sintab, b_y1);
} else {
b_dobluesteinfft(b, N2blue, x->size[1], costab, sintab, sintabinv, b_y1);
}
}
emxFree_real_T(&sintabinv);
emxFree_real_T(&sintab);
emxFree_real_T(&costab);
emxFree_real_T(&costab1q);
emxFree_creal_T(&b);
i = y->size[0] * y->size[1];
y->size[0] = b_y1->size[1];
y->size[1] = b_y1->size[0];
emxEnsureCapacity_creal_T(y, i);
nd2 = b_y1->size[0];
for (i = 0; i < nd2; i++) {
n2 = b_y1->size[1];
for (i1 = 0; i1 < n2; i1++) {
y->data[i1 + y->size[0] * i] = b_y1->data[i + b_y1->size[0] * i1];
}
}
emxFree_creal_T(&b_y1);
}
//
// File trailer for ifft.cpp
//
// [EOF]
//

26
Classes/ifft.h Normal file
View File

@@ -0,0 +1,26 @@
//
// File: ifft.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef IFFT_H
#define IFFT_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern void ifft(const emxArray_creal_T *x, emxArray_creal_T *y);
#endif
//
// File trailer for ifft.h
//
// [EOF]
//

51
Classes/log2.cpp Normal file
View File

@@ -0,0 +1,51 @@
//
// File: log2.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "log2.h"
#include "cwt.h"
#include "rt_nonfinite.h"
#include <cmath>
#include <math.h>
#include <string.h>
// Function Definitions
//
// Arguments : double x
// Return Type : double
//
double b_log2(double x)
{
double f;
double t;
int eint;
if (x == 0.0) {
f = rtMinusInf;
} else if (x < 0.0) {
f = rtNaN;
} else if ((!rtIsInf(x)) && (!rtIsNaN(x))) {
t = frexp(x, &eint);
if (t == 0.5) {
f = static_cast<double>(eint) - 1.0;
} else if ((eint == 1) && (t < 0.75)) {
f = std::log(2.0 * t) / 0.69314718055994529;
} else {
f = std::log(t) / 0.69314718055994529 + static_cast<double>(eint);
}
} else {
f = x;
}
return f;
}
//
// File trailer for log2.cpp
//
// [EOF]
//

26
Classes/log2.h Normal file
View File

@@ -0,0 +1,26 @@
//
// File: log2.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef LOG2_H
#define LOG2_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern double b_log2(double x);
#endif
//
// File trailer for log2.h
//
// [EOF]
//

3
Classes/main.cpp
View File

@@ -1,9 +1,12 @@
#include <QtWidgets/QApplication>
#include "Loading.h"
#include "Reconstruction.h"
#include <windows.h>
int main(int argc, char *argv[])
{
AllocConsole();//分配控制台
freopen("CONOUT$", "w+t", stdout);//向控制台输出
QApplication a(argc, argv);
Loading l;
l.show();

54
Classes/rtGetInf.cpp Normal file
View File

@@ -0,0 +1,54 @@
//
// File: rtGetInf.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Abstract:
// MATLAB for code generation function to initialize non-finite, Inf and MinusInf
#include "rtGetInf.h"
// Function: rtGetInf ==================================================================
// Abstract:
// Initialize rtInf needed by the generated code.
real_T rtGetInf(void)
{
return rtInf;
}
// Function: rtGetInfF =================================================================
// Abstract:
// Initialize rtInfF needed by the generated code.
real32_T rtGetInfF(void)
{
return rtInfF;
}
// Function: rtGetMinusInf =============================================================
// Abstract:
// Initialize rtMinusInf needed by the generated code.
real_T rtGetMinusInf(void)
{
return rtMinusInf;
}
// Function: rtGetMinusInfF ============================================================
// Abstract:
// Initialize rtMinusInfF needed by the generated code.
real32_T rtGetMinusInfF(void)
{
return rtMinusInfF;
}
//
// File trailer for rtGetInf.cpp
//
// [EOF]

33
Classes/rtGetInf.h Normal file
View File

@@ -0,0 +1,33 @@
//
// File: rtGetInf.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
#ifndef RTGETINF_H
#define RTGETINF_H
#include "rtwtypes.h"
#include "rt_nonfinite.h"
#ifdef __cplusplus
extern "C" {
#endif
extern real_T rtGetInf(void);
extern real32_T rtGetInfF(void);
extern real_T rtGetMinusInf(void);
extern real32_T rtGetMinusInfF(void);
#ifdef __cplusplus
}
#endif
#endif
//
// File trailer for rtGetInf.h
//
// [EOF]

38
Classes/rtGetNaN.cpp Normal file
View File

@@ -0,0 +1,38 @@
//
// File: rtGetNaN.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Abstract:
// MATLAB for code generation function to initialize non-finite, NaN
#include "rtGetNaN.h"
// Function: rtGetNaN ======================================================================
// Abstract:
// Initialize rtNaN needed by the generated code.
// NaN is initialized as non-signaling. Assumes IEEE.
real_T rtGetNaN(void)
{
return rtNaN;
}
// Function: rtGetNaNF =====================================================================
// Abstract:
// Initialize rtNaNF needed by the generated code.
// NaN is initialized as non-signaling. Assumes IEEE.
real32_T rtGetNaNF(void)
{
return rtNaNF;
}
//
// File trailer for rtGetNaN.cpp
//
// [EOF]

32
Classes/rtGetNaN.h Normal file
View File

@@ -0,0 +1,32 @@
//
// File: rtGetNaN.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
#ifndef RTGETNAN_H
#define RTGETNAN_H
#include <stddef.h>
#include "rtwtypes.h"
#include "rt_nonfinite.h"
#ifdef __cplusplus
extern "C" {
#endif
extern real_T rtGetNaN(void);
extern real32_T rtGetNaNF(void);
#ifdef __cplusplus
}
#endif
#endif
//
// File trailer for rtGetNaN.h
//
// [EOF]

25
Classes/rt_defines.h Normal file
View File

@@ -0,0 +1,25 @@
//
// File: rt_defines.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef RT_DEFINES_H
#define RT_DEFINES_H
static const double RT_PI = 3.14159265358979323846;
static const float RT_PIF = 3.1415927F;
static const double RT_LN_10 = 2.30258509299404568402;
static const float RT_LN_10F = 2.3025851F;
static const double RT_LOG10E = 0.43429448190325182765;
static const float RT_LOG10EF = 0.43429449F;
static const double RT_E = 2.7182818284590452354;
static const float RT_EF = 2.7182817F;
#endif
//
// File trailer for rt_defines.h
//
// [EOF]
//

79
Classes/rt_nonfinite.cpp Normal file
View File

@@ -0,0 +1,79 @@
//
// File: rt_nonfinite.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Abstract:
// MATLAB for code generation function to initialize non-finites,
// (Inf, NaN and -Inf).
#include "rt_nonfinite.h"
#include <cmath>
#include <limits>
real_T rtInf;
real_T rtMinusInf;
real_T rtNaN;
real32_T rtInfF;
real32_T rtMinusInfF;
real32_T rtNaNF;
// Function: rt_InitInfAndNaN ==================================================
// Abstract:
// Initialize the rtInf, rtMinusInf, and rtNaN needed by the
// generated code. NaN is initialized as non-signaling. Assumes IEEE.
void rt_InitInfAndNaN()
{
rtNaN = std::numeric_limits<real_T>::quiet_NaN();
rtNaNF = std::numeric_limits<real32_T>::quiet_NaN();
rtInf = std::numeric_limits<real_T>::infinity();
rtInfF = std::numeric_limits<real32_T>::infinity();
rtMinusInf = -std::numeric_limits<real_T>::infinity();
rtMinusInfF = -std::numeric_limits<real32_T>::infinity();
}
// Function: rtIsInf ==================================================
// Abstract:
// Test if value is infinite
boolean_T rtIsInf(real_T value)
{
return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
}
// Function: rtIsInfF =================================================
// Abstract:
// Test if single-precision value is infinite
boolean_T rtIsInfF(real32_T value)
{
return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
}
// Function: rtIsNaN ==================================================
// Abstract:
// Test if value is not a number
boolean_T rtIsNaN(real_T value)
{
return ((value!=value)? 1U : 0U);
}
// Function: rtIsNaNF =================================================
// Abstract:
// Test if single-precision value is not a number
boolean_T rtIsNaNF(real32_T value)
{
return ((value!=value)? 1U : 0U);
}
//
// File trailer for rt_nonfinite.cpp
//
// [EOF]

39
Classes/rt_nonfinite.h Normal file
View File

@@ -0,0 +1,39 @@
//
// File: rt_nonfinite.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
#ifndef RT_NONFINITE_H
#define RT_NONFINITE_H
#include "rtwtypes.h"
#ifdef __cplusplus
extern "C" {
#endif
extern real_T rtInf;
extern real_T rtMinusInf;
extern real_T rtNaN;
extern real32_T rtInfF;
extern real32_T rtMinusInfF;
extern real32_T rtNaNF;
extern void rt_InitInfAndNaN();
extern boolean_T rtIsInf(real_T value);
extern boolean_T rtIsInfF(real32_T value);
extern boolean_T rtIsNaN(real_T value);
extern boolean_T rtIsNaNF(real32_T value);
#ifdef __cplusplus
}
#endif
#endif
//
// File trailer for rt_nonfinite.h
//
// [EOF]

43
Classes/rtwtypes.h Normal file
View File

@@ -0,0 +1,43 @@
/*
* File: rtwtypes.h
*
* MATLAB Coder version : 4.3
* C/C++ source code generated on : 30-Mar-2020 11:59:32
*/
#ifndef RTWTYPES_H
#define RTWTYPES_H
/*=======================================================================*
* Fixed width word size data types: *
* int64_T - signed 64 bit integers *
* uint64_T - unsigned 64 bit integers *
*=======================================================================*/
#if defined(__APPLE__)
# ifndef INT64_T
# define INT64_T long
# define FMT64 "l"
# if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
# define INT_TYPE_64_IS_LONG
# endif
# endif
#endif
#if defined(__APPLE__)
# ifndef UINT64_T
# define UINT64_T unsigned long
# define FMT64 "l"
# if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
# define INT_TYPE_64_IS_LONG
# endif
# endif
#endif
#include "tmwtypes.h"
#endif
/*
* File trailer for rtwtypes.h
*
* [EOF]
*/

458
Classes/structured_light.cpp Normal file
View File

@@ -0,0 +1,458 @@
#include "structured_light.h"
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
namespace sl
{
const float PIXEL_UNCERTAIN = std::numeric_limits<float>::quiet_NaN();
const unsigned short BIT_UNCERTAIN = 0xffff;
}
bool sl::decode_pattern(const std::vector<cv::Mat> & images, cv::Mat & pattern_image, cv::Mat & min_max_image, cv::Size const& projector_size, unsigned flags, const cv::Mat & direct_light, unsigned m)
{
// for(int i = 0; i < images.size(); i++) {
// std::cout<<images[i].cols<<" "<<images[i].rows<<std::endl;
// }
bool binary = (flags & GrayPatternDecode)!=GrayPatternDecode;
bool robust = (flags & RobustDecode)==RobustDecode;
std::cout << " --- decode_pattern START ---\n";
//delete previous data
pattern_image = cv::Mat();
min_max_image = cv::Mat();
bool init = true;
std::cout << "Decode: " << (binary?"Binary ":"Gray ")
<< (robust?"Robust ":"")
<< std::endl;
unsigned int total_images = static_cast<unsigned int>(images.size());
int v_bits = 1;
int h_bits = 1;
for (int i=(1<<v_bits); i<projector_size.height; i=(1<<v_bits)) { v_bits++; }
for (int i=(1<<h_bits); i<projector_size.width; i=(1<<h_bits)) { h_bits++; }
// std::cout<<2+4*total_bits<<" "<<total_images<<std::endl;
if (2+2*v_bits+2*h_bits!=total_images)
{ //error
std::cout << "[sl::decode_pattern] ERROR: cannot detect pattern and bit count from image set.\n";
return false;
}
const unsigned bit_count[] = {0, h_bits, v_bits}; //pattern bits
const unsigned set_size[] = {1, h_bits, v_bits}; //number of image pairs
const unsigned COUNT = 2*(set_size[0]+set_size[1]+set_size[2]); //total image count
//??????
const int pattern_offset[2] = {((1<<h_bits)-projector_size.width)/2, ((1<<v_bits)-projector_size.height)/2};
if (images.size()<COUNT)
{ //error
std::cout << "Image list size does not match set size, please supply exactly " << COUNT << " image names.\n";
return false;
}
//load every image pair and compute the maximum, minimum, and bit code
unsigned set = 0;
unsigned current = 0;
for (unsigned t=0; t<COUNT; t+=2, current++)
{
if (current==set_size[set])
{
set++;
current = 0;
}
if (set==0)
{ //skip
continue;
}
unsigned bit = bit_count[set] - current - 1; //current bit: from 0 to (bit_count[set]-1)
unsigned channel = set - 1;
//load images
const cv::Mat & gray_image1 = get_gray_image(images[t+0]);
// std::cout<<t<<" "<<images[t].cols<<" "<<images[t].rows<<" "<<images[t].channels()<<std::endl;
if (gray_image1.rows<1)
{
std::cout << "Failed to load " << t+0 <<"th image"<< std::endl;
return false;
}
const cv::Mat & gray_image2 = get_gray_image(images[t+1]);
if (gray_image2.rows<1)
{
std::cout << "Failed to load " << t+1 << "th image" << std::endl;
return false;
}
//initialize data structures
if (init)
{
//sanity check
if (gray_image1.size()!=gray_image2.size())
{ //different size
std::cout << " --> Initial images have different size: \n";
return false;
}
if (robust && gray_image1.size()!=direct_light.size())
{ //different size
std::cout << " --> Direct Component image has different size: \n";
return false;
}
pattern_image = cv::Mat(gray_image1.size(), CV_32FC2);
min_max_image = cv::Mat(gray_image1.size(), CV_8UC2);
}
//sanity check
if (gray_image1.size()!=pattern_image.size())
{ //different size
std::cout << " --> Image 1 has different size, image pair " << t << " (skipped!)\n";
continue;
}
if (gray_image2.size()!=pattern_image.size())
{ //different size
std::cout << " --> Image 2 has different size, image pair " << t << " (skipped!)\n";
continue;
}
//compare
for (int h=0; h<pattern_image.rows; h++)
{
const unsigned char * row1 = gray_image1.ptr<unsigned char>(h);
const unsigned char * row2 = gray_image2.ptr<unsigned char>(h);
const cv::Vec2b * row_light = (robust ? direct_light.ptr<cv::Vec2b>(h) : NULL);
cv::Vec2f * pattern_row = pattern_image.ptr<cv::Vec2f>(h);
cv::Vec2b * min_max_row = min_max_image.ptr<cv::Vec2b>(h);
for (int w=0; w<pattern_image.cols; w++)
{
cv::Vec2f & pattern = pattern_row[w];
cv::Vec2b & min_max = min_max_row[w];
unsigned char value1 = row1[w];
unsigned char value2 = row2[w];
if (init)
{
pattern[0] = 0.f; //vertical
pattern[1] = 0.f; //horizontal
}
//min/max
if (init || value1<min_max[0] || value2<min_max[0])
{
min_max[0] = (value1<value2?value1:value2);
}
if (init || value1>min_max[1] || value2>min_max[1])
{
min_max[1] = (value1>value2?value1:value2);
}
if (!robust)
{ // [simple] pattern bit assignment
if (value1>value2)
{ //set bit n to 1
pattern[channel] += (1<<bit);
}
}
else
{ // [robust] pattern bit assignment
if (row_light && (init || pattern[channel]!=PIXEL_UNCERTAIN))
{
const cv::Vec2b & L = row_light[w];
unsigned short p = get_robust_bit(value1, value2, L[0], L[1], m);
if (p==BIT_UNCERTAIN)
{
pattern[channel] = PIXEL_UNCERTAIN;
}
else
{
pattern[channel] += (p<<bit);
}
}
}
} //for each column
} //for each row
init = false;
} //for all image pairs
if (!binary)
{ //not binary... it must be gray code
convert_pattern(pattern_image, projector_size, pattern_offset, binary);
}
std::cout << " --- decode_pattern END ---"<<std::endl;
return true;
}
unsigned short sl::get_robust_bit(unsigned value1, unsigned value2, unsigned Ld, unsigned Lg, unsigned m)
{
if (Ld < m)
{
return BIT_UNCERTAIN;
}
if (Ld>Lg)
{
return (value1>value2 ? 1 : 0);
}
if (value1<=Ld && value2>=Lg)
{
return 0;
}
if (value1>=Lg && value2<=Ld)
{
return 1;
}
return BIT_UNCERTAIN;
}
void sl::convert_pattern(cv::Mat & pattern_image, cv::Size const& projector_size, const int offset[2], bool binary)
{
if (pattern_image.rows==0)
{ //no pattern image
return;
}
if (pattern_image.type()!=CV_32FC2)
{
return;
}
if (binary)
{
std::cout << "Converting binary code to gray\n";
}
else
{
std::cout << "Converting gray code to binary\n";
}
for (int h=0; h<pattern_image.rows; h++)
{
cv::Vec2f * pattern_row = pattern_image.ptr<cv::Vec2f>(h);
for (int w=0; w<pattern_image.cols; w++)
{
cv::Vec2f & pattern = pattern_row[w];
if (binary)
{
if (!INVALID(pattern[0]))
{
int p = static_cast<int>(pattern[0]);
pattern[0] = binaryToGray(p, offset[0]) + (pattern[0] - p);
}
if (!INVALID(pattern[1]))
{
int p = static_cast<int>(pattern[1]);
pattern[1] = binaryToGray(p, offset[1]) + (pattern[1] - p);
}
}
else
{
if (!INVALID(pattern[0]))
{
int p = static_cast<int>(pattern[0]);
int code = grayToBinary(p, offset[0]);
if (code<0) {code = 0;}
else if (code>=projector_size.width) {code = projector_size.width - 1;}
pattern[0] = code + (pattern[0] - p);
}
if (!INVALID(pattern[1]))
{
int p = static_cast<int>(pattern[1]);
int code = grayToBinary(p, offset[1]);
if (code<0) {code = 0;}
else if (code>=projector_size.height) {code = projector_size.height - 1;}
pattern[1] = code + (pattern[1] - p);
}
}
}
}
}
cv::Mat sl::estimate_direct_light(const std::vector<cv::Mat> & images, float b)
{
static const unsigned COUNT = 10; // max number of images
unsigned count = static_cast<int>(images.size());
if (count<1)
{ //no images
return cv::Mat();
}
std::cout << " --- estimate_direct_light START ---\n";
if (count>COUNT)
{
count = COUNT;
std::cout << "WARNING: Using only " << COUNT << " of " << count << std::endl;
}
for (unsigned i=0; i<count; i++)
{
if (images.at(i).type()!=CV_8UC1)
{ //error
std::cout << "Gray images required\n";
return cv::Mat();
}
}
cv::Size size = images.at(0).size();
//initialize direct light image
cv::Mat direct_light(size, CV_8UC2);
double b1 = 1.0/(1.0 - b);
double b2 = 2.0/(1.0 - b*1.0*b);
for (unsigned h=0; static_cast<int>(h)<size.height; h++)
{
unsigned char const* row[COUNT];
for (unsigned i=0; i<count; i++)
{
row[i] = images.at(i).ptr<unsigned char>(h);
}
cv::Vec2b * row_light = direct_light.ptr<cv::Vec2b>(h);
for (unsigned w=0; static_cast<int>(w)<size.width; w++)
{
unsigned Lmax = row[0][w];
unsigned Lmin = row[0][w];
for (unsigned i=0; i<count; i++)
{
if (Lmax<row[i][w]) Lmax = row[i][w];
if (Lmin>row[i][w]) Lmin = row[i][w];
}
int Ld = static_cast<int>(b1*(Lmax - Lmin) + 0.5);
int Lg = static_cast<int>(b2*(Lmin - b*Lmax) + 0.5);
row_light[w][0] = (Lg>0 ? static_cast<unsigned>(Ld) : Lmax);
row_light[w][1] = (Lg>0 ? static_cast<unsigned>(Lg) : 0);
//std::cout << "Ld=" << (int)row_light[w][0] << " iTotal=" <<(int) row_light[w][1] << std::endl;
}
}
std::cout << " --- estimate_direct_light END ---\n";
return direct_light;
}
cv::Mat sl::get_gray_image(cv::Mat img)
{
if (img.channels() > 1)
{
//gray scale
cv::Mat gray_image;
cvtColor(img, gray_image, cv::COLOR_BGR2GRAY);
return gray_image;
}
cv::Mat res = img.clone();
return res;
}
/* From Wikipedia: http://en.wikipedia.org/wiki/Gray_code
The purpose of this function is to convert an unsigned
binary number to reflected binary Gray code.
*/
static unsigned util_binaryToGray(unsigned num)
{
return (num>>1) ^ num;
}
/* From Wikipedia: http://en.wikipedia.org/wiki/Gray_code
The purpose of this function is to convert a reflected binary
Gray code number to a binary number.
*/
static unsigned util_grayToBinary(unsigned num, unsigned numBits)
{
for (unsigned shift = 1; shift < numBits; shift <<= 1)
{
num ^= num >> shift;
}
return num;
}
int sl::binaryToGray(int value) {return util_binaryToGray(value);}
inline int sl::binaryToGray(int value, unsigned offset) {return util_binaryToGray(value + offset);}
inline int sl::grayToBinary(int value, unsigned offset) {return (util_grayToBinary(value, 32) - offset);}
cv::Mat sl::colorize_pattern(const cv::Mat & pattern_image, unsigned set, float max_value)
{
if (pattern_image.rows==0)
{ //empty image
return cv::Mat();
}
if (pattern_image.type()!=CV_32FC2)
{ //invalid image type
return cv::Mat();
}
if (set!=0 && set!=1)
{
return cv::Mat();
}
cv::Mat image(pattern_image.size(), CV_8UC3);
float max_t = max_value;
float n = 4.f;
float dt = 255.f/n;
for (int h=0; h<pattern_image.rows; h++)
{
const cv::Vec2f * row1 = pattern_image.ptr<cv::Vec2f>(h);
cv::Vec3b * row2 = image.ptr<cv::Vec3b>(h);
for (int w=0; w<pattern_image.cols; w++)
{
if (row1[w][set]>max_value || INVALID(row1[w][set]))
{ //invalid value: use grey
row2[w] = cv::Vec3b(128, 128, 128);
continue;
}
//display
float t = row1[w][set]*255.f/max_t;
float c1 = 0.f, c2 = 0.f, c3 = 0.f;
if (t<=1.f*dt)
{ //black -> red
float c = n*(t-0.f*dt);
c1 = c; //0-255
c2 = 0.f; //0
c3 = 0.f; //0
}
else if (t<=2.f*dt)
{ //red -> red,green
float c = n*(t-1.f*dt);
c1 = 255.f; //255
c2 = c; //0-255
c3 = 0.f; //0
}
else if (t<=3.f*dt)
{ //red,green -> green
float c = n*(t-2.f*dt);
c1 = 255.f-c; //255-0
c2 = 255.f; //255
c3 = 0.f; //0
}
else if (t<=4.f*dt)
{ //green -> blue
float c = n*(t-3.f*dt);
c1 = 0.f; //0
c2 = 255.f-c; //255-0
c3 = c; //0-255
}
row2[w] = cv::Vec3b(static_cast<uchar>(c3), static_cast<uchar>(c2), static_cast<uchar>(c1));
}
}
return image;
}

38
Classes/structured_light.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef __STRUCTURED_LIGHT_HPP__
#define __STRUCTURED_LIGHT_HPP__
#include <opencv2/core/core.hpp>
#ifndef _MSC_VER
# ifndef _isnan
# include <math.h>
# define _isnan std::isnan
# endif
#endif
namespace sl
{
enum DecodeFlags {SimpleDecode = 0x00, GrayPatternDecode = 0x01, RobustDecode = 0x02};
extern const float PIXEL_UNCERTAIN;
extern const unsigned short BIT_UNCERTAIN;
bool decode_pattern(const std::vector<cv::Mat> & images, cv::Mat & pattern_image, cv::Mat & min_max_image, cv::Size const& projector_size,
unsigned flags = SimpleDecode, const cv::Mat & direct_light = cv::Mat(), unsigned m = 5);
unsigned short get_robust_bit(unsigned value1, unsigned value2, unsigned Ld, unsigned Lg, unsigned m);
void convert_pattern(cv::Mat & pattern_image, cv::Size const& projector_size, const int offset[2], bool binary);
cv::Mat estimate_direct_light(const std::vector<cv::Mat> & images, float b);
cv::Mat get_gray_image(cv::Mat img);
static inline bool INVALID(float value) {return _isnan(value)>0;}
static inline bool INVALID(const cv::Vec2f & pt) {return _isnan(pt[0]) || _isnan(pt[1]);}
static inline bool INVALID(const cv::Vec3f & pt) {return _isnan(pt[0]) || _isnan(pt[1]) || _isnan(pt[2]);}
int binaryToGray(int value);
inline int binaryToGray(int value, unsigned offset);
inline int grayToBinary(int value, unsigned offset);
cv::Mat colorize_pattern(const cv::Mat & pattern_image, unsigned set, float max_value);
};
#endif //__STRUCTURED_LIGHT_HPP__

757
Classes/tmwtypes.h Normal file
View File

@@ -0,0 +1,757 @@
/*
* @(#)tmwtypes.h generated by: makeheader 5.1.3 Mon Jan 8 22:14:40 2007
*
* built from: ../../src/include/copyright.h
* ../../src/include/tmwtypes.h
*/
#if defined(_MSC_VER) || __GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ > 3)
#pragma once
#endif
#ifndef tmwtypes_h
#define tmwtypes_h
/*
* Copyright 1984-2007 The MathWorks, Inc.
*/
/* Copyright 1995-2006 The MathWorks, Inc. */
#ifndef __TMWTYPES__
#define __TMWTYPES__
/*
* File : tmwtypes.h
* Abstract:
* Data types for use with MATLAB/SIMULINK and the Real-Time Workshop.
*
* When compiling stand-alone model code, data types can be overridden
* via compiler switches.
*
* Define NO_FLOATS to eliminate reference to real_T, etc.
*/
#include <limits.h>
#ifdef __APPLE_CC__
#include <stdbool.h>
#endif
#define LOGICAL_IS_A_TYPE
#define SPARSE_GENERALIZATION
#ifdef NO_FLOATS
# define double double_not_allowed
# define float float_not_allowed
#endif /*NO_FLOATS*/
#ifndef NO_FLOATS
#ifndef __MWERKS__
# ifdef __STDC__
# include <float.h>
# else
# define FLT_MANT_DIG 24
# define DBL_MANT_DIG 53
# endif
#endif
#endif /*NO_FLOATS*/
/*
* The following data types cannot be overridden when building MEX files.
*/
#ifdef MATLAB_MEX_FILE
# undef CHARACTER_T
# undef INTEGER_T
# undef BOOLEAN_T
# undef REAL_T
# undef TIME_T
#endif
/*
* The uchar_T, ushort_T and ulong_T types are needed for compilers which do
* not allow defines to be specified, at the command line, with spaces in them.
*/
typedef unsigned char uchar_T;
typedef unsigned short ushort_T;
typedef unsigned long ulong_T;
/*=======================================================================*
* Fixed width word size data types: *
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
* real32_T, real64_T - 32 and 64 bit floating point numbers *
*=======================================================================*/
/* When used with Real Time Workshop generated code, this
* header file can be used with a variety of compilers.
*
* The compiler could be for an 8 bit embedded processor that
* only had 8 bits per integer and 16 bits per long.
* In that example, a 32 bit integer size is not even available.
* This header file should be robust to that.
*
* For the case of an 8 bit processor, the preprocessor
* may be limited to 16 bit math like its target. That limitation
* would mean that 32 bit comparisons can't be done accurately.
* To increase robustness to this, comparisons are done against
* smaller values first. An inaccurate 32 bit comparison isn't
* attempted if the 16 bit comparison has already succeeded.
*
* Limitations on preprocessor math can also be stricter than
* for the target. There are known cases where a compiler
* targeting processors with 64 bit longs can't do accurate
* preprocessor comparisons on more than 32 bits.
*/
/* Determine the number of bits for int, long, short, and char.
* If one fails to be determined, set the number of bits to -1
*/
#ifndef TMW_BITS_PER_INT
# if INT_MAX == 0x7FL
# define TMW_BITS_PER_INT 8
# elif INT_MAX == 0x7FFFL
# define TMW_BITS_PER_INT 16
# elif INT_MAX == 0x7FFFFFFFL
# define TMW_BITS_PER_INT 32
# else
# define TMW_BITS_PER_INT -1
# endif
#endif
#ifndef TMW_BITS_PER_LONG
# if LONG_MAX == 0x7FL
# define TMW_BITS_PER_LONG 8
# elif LONG_MAX == 0x7FFFL
# define TMW_BITS_PER_LONG 16
# elif LONG_MAX == 0x7FFFFFFFL
# define TMW_BITS_PER_LONG 32
# else
# define TMW_BITS_PER_LONG -1
# endif
#endif
#ifndef TMW_BITS_PER_SHRT
# if SHRT_MAX == 0x7FL
# define TMW_BITS_PER_SHRT 8
# elif SHRT_MAX == 0x7FFFL
# define TMW_BITS_PER_SHRT 16
# elif SHRT_MAX == 0x7FFFFFFFL
# define TMW_BITS_PER_SHRT 32
# else
# define TMW_BITS_PER_SHRT -1
# endif
#endif
#ifndef TMW_BITS_PER_SCHAR
# if SCHAR_MAX == 0x7FL
# define TMW_BITS_PER_SCHAR 8
# elif SCHAR_MAX == 0x7FFFL
# define TMW_BITS_PER_SCHAR 16
# elif SCHAR_MAX == 0x7FFFFFFFL
# define TMW_BITS_PER_SCHAR 32
# else
# define TMW_BITS_PER_SCHAR -1
# endif
#endif
#ifndef TMW_CHAR_SIGNED
# if SCHAR_MAX == CHAR_MAX
# define TMW_CHAR_SIGNED 1
# else
# define TMW_CHAR_SIGNED 0
# endif
#endif
/* It is common for one or more of the integer types
* to be the same size. For example, on many embedded
* processors, both shorts and ints are 16 bits. On
* processors used for workstations, it is quite common
* for both int and long to be 32 bits.
* When there is more than one choice for typdef'ing
* a portable type like int16_T or uint32_T, in
* concept, it should not matter which choice is made.
* However, some style guides and some code checking
* tools do identify and complain about seemingly
* irrelevant differences. For example, a code
* checking tool may complain about an implicit
* conversion from int to short even though both
* are 16 bits. To reduce these types of
* complaints, it is best to make int the
* preferred choice when more than one is available.
*/
#ifndef INT8_T
# if TMW_BITS_PER_INT == 8
# define INT8_T int
# elif TMW_BITS_PER_LONG == 8
# define INT8_T long
# elif TMW_BITS_PER_SCHAR == 8
# if TMW_CHAR_SIGNED
# define INT8_T char
# else
# define INT8_T signed char
# endif
# elif TMW_BITS_PER_SHRT == 8
# define INT8_T short
# endif
#endif
#ifdef INT8_T
typedef INT8_T int8_T;
#endif
#ifndef UINT8_T
# if TMW_BITS_PER_INT == 8
# define UINT8_T unsigned int
# elif TMW_BITS_PER_LONG == 8
# define UINT8_T unsigned long
# elif TMW_BITS_PER_SCHAR == 8
# if TMW_CHAR_SIGNED
# define UINT8_T unsigned char
# else
# define UINT8_T char
# endif
# elif TMW_BITS_PER_SHRT == 8
# define UINT8_T unsigned short
# endif
#endif
#ifdef UINT8_T
typedef UINT8_T uint8_T;
#endif
#ifndef INT16_T
# if TMW_BITS_PER_INT == 16
# define INT16_T int
# elif TMW_BITS_PER_LONG == 16
# define INT16_T long
# elif TMW_BITS_PER_SCHAR == 16
# if TMW_CHAR_SIGNED
# define INT16_T char
# else
# define INT16_T signed char
# endif
# elif TMW_BITS_PER_SHRT == 16
# define INT16_T short
# endif
#endif
#ifdef INT16_T
typedef INT16_T int16_T;
#endif
#ifndef UINT16_T
# if TMW_BITS_PER_INT == 16
# define UINT16_T unsigned int
# elif TMW_BITS_PER_LONG == 16
# define UINT16_T unsigned long
# elif TMW_BITS_PER_SCHAR == 16
# if TMW_CHAR_SIGNED
# define UINT16_T unsigned char
# else
# define UINT16_T char
# endif
# elif TMW_BITS_PER_SHRT == 16
# define UINT16_T unsigned short
# endif
#endif
#ifdef UINT16_T
typedef UINT16_T uint16_T;
#endif
#ifndef INT32_T
# if TMW_BITS_PER_INT == 32
# define INT32_T int
# elif TMW_BITS_PER_LONG == 32
# define INT32_T long
# elif TMW_BITS_PER_SCHAR == 32
# if TMW_CHAR_SIGNED
# define INT32_T char
# else
# define INT32_T signed char
# endif
# elif TMW_BITS_PER_SHRT == 32
# define INT32_T short
# endif
#endif
#ifdef INT32_T
typedef INT32_T int32_T;
#endif
#ifndef UINT32_T
# if TMW_BITS_PER_INT == 32
# define UINT32_T unsigned int
# elif TMW_BITS_PER_LONG == 32
# define UINT32_T unsigned long
# elif TMW_BITS_PER_SCHAR == 32
# if TMW_CHAR_SIGNED
# define UINT32_T unsigned char
# else
# define UINT32_T char
# endif
# elif TMW_BITS_PER_SHRT == 32
# define UINT32_T unsigned short
# endif
#endif
#ifdef UINT32_T
typedef UINT32_T uint32_T;
#endif
/* The following is used to emulate smaller integer types when only
* larger types are available. For example, compilers for TI C3x/C4x DSPs
* define char and short to be 32 bits, so 8 and 16 bits are not directly
* available. This target is commonly used with RTW rapid prototyping.
* Other DSPs define char to be 16 bits, so 8 bits is not directly
* available.
*/
#ifndef INT8_T
# ifdef INT16_T
typedef INT16_T int8_T;
# else
# ifdef INT32_T
typedef INT32_T int8_T;
# endif
# endif
#endif
#ifndef UINT8_T
# ifdef UINT16_T
typedef UINT16_T uint8_T;
# else
# ifdef UINT32_T
typedef UINT32_T uint8_T;
# endif
# endif
#endif
#ifndef INT16_T
# ifdef INT32_T
typedef INT32_T int16_T;
# endif
#endif
#ifndef UINT16_T
# ifdef UINT32_T
typedef UINT32_T uint16_T;
# endif
#endif
#ifndef NO_FLOATS
#ifndef REAL32_T
# ifndef __MWERKS__
# if FLT_MANT_DIG >= 23
# define REAL32_T float
# endif
# else
# define REAL32_T float
# endif
#endif
#ifdef REAL32_T
typedef REAL32_T real32_T;
#endif
#ifndef REAL64_T
# ifndef __MWERKS__
# if DBL_MANT_DIG >= 52
# define REAL64_T double
# endif
# else
# define REAL64_T double
# endif
#endif
#ifdef REAL64_T
typedef REAL64_T real64_T;
#endif
#endif /* NO_FLOATS*/
/*=======================================================================*
* Fixed width word size data types: *
* int64_T - signed 64 bit integers *
* uint64_T - unsigned 64 bit integers *
*=======================================================================*/
#ifndef INT64_T
# if defined(__alpha) || defined(__sparcv9) || defined(__ia64) || \
defined(__ia64__) || defined(__x86_64__) || defined(__LP64__)
# define INT64_T long
# define FMT64 "l"
# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
|| (defined(__WATCOMC__) && __WATCOMC__ >= 1100)
# define INT64_T __int64
# define FMT64 "I64"
# elif defined(__GNUC__) || defined(__hpux) || defined(__sun) \
|| defined(TMW_ENABLE_INT64)
# define INT64_T long long
# define FMT64 "ll"
# endif
#endif
#if defined(INT64_T)
# if (__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9))
__extension__
# endif
typedef INT64_T int64_T;
#endif
#ifndef UINT64_T
# if defined(__alpha) || defined(__sparcv9) || defined(__ia64) || \
defined(__ia64__) || defined(__x86_64__) || defined(__LP64__)
# define UINT64_T unsigned long
# define FMT64 "l"
# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
|| (defined(__WATCOMC__) && __WATCOMC__ >= 1100)
# define UINT64_T unsigned __int64
# define FMT64 "I64"
# elif defined(__GNUC__) || defined(__hpux) || defined(__sun) \
|| defined(TMW_ENABLE_INT64)
# define UINT64_T unsigned long long
# define FMT64 "ll"
# endif
#endif
#if defined(UINT64_T)
# if (__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9))
__extension__
# endif
typedef UINT64_T uint64_T;
#endif
/*===========================================================================*
* Format string modifiers for using size_t variables in printf statements. *
*===========================================================================*/
#ifndef FMT_SIZE_T
# if defined( __GNUC__ ) || defined(_STDC_C99)
# define FMT_SIZE_T "z"
# elif defined (__WATCOMC__)
# define FMT_SIZE_T "l"
# elif defined (_WIN32 )
# define FMT_SIZE_T "I"
# else
# define FMT_SIZE_T "l"
# endif
#endif
/*===========================================================================*
* General or logical data types where the word size is not guaranteed. *
* real_T - possible settings include real32_T or real64_T *
* time_T - possible settings include real64_T or uint32_T *
* boolean_T *
* char_T *
* int_T *
* uint_T *
* byte_T *
*===========================================================================*/
#ifndef NO_FLOATS
#ifndef REAL_T
# ifdef REAL64_T
# define REAL_T real64_T
# else
# ifdef REAL32_T
# define REAL_T real32_T
# endif
# endif
#endif
#ifdef REAL_T
typedef REAL_T real_T;
#endif
#ifndef TIME_T
# ifdef REAL_T
# define TIME_T real_T
# endif
#endif
#ifdef TIME_T
typedef TIME_T time_T;
#endif
#endif /* NO_FLOATS */
#ifndef BOOLEAN_T
# if defined(UINT8_T)
# define BOOLEAN_T UINT8_T
# else
# define BOOLEAN_T unsigned int
# endif
#endif
typedef BOOLEAN_T boolean_T;
#ifndef CHARACTER_T
# define CHARACTER_T char
#endif
typedef CHARACTER_T char_T;
#ifndef INTEGER_T
# define INTEGER_T int
#endif
typedef INTEGER_T int_T;
#ifndef UINTEGER_T
# define UINTEGER_T unsigned
#endif
typedef UINTEGER_T uint_T;
#ifndef BYTE_T
# define BYTE_T unsigned char
#endif
typedef BYTE_T byte_T;
/*===========================================================================*
* Define Complex Structures *
*===========================================================================*/
#ifndef NO_FLOATS
#ifndef CREAL32_T
# ifdef REAL32_T
typedef struct {
real32_T re, im;
} creal32_T;
# define CREAL32_T creal32_T
# endif
#endif
#ifndef CREAL64_T
# ifdef REAL64_T
typedef struct {
real64_T re, im;
} creal64_T;
# define CREAL64_T creal64_T
# endif
#endif
#ifndef CREAL_T
# ifdef REAL_T
typedef struct {
real_T re, im;
} creal_T;
# define CREAL_T creal_T
# endif
#endif
#endif /* NO_FLOATS */
#ifndef CINT8_T
# ifdef INT8_T
typedef struct {
int8_T re, im;
} cint8_T;
# define CINT8_T cint8_T
# endif
#endif
#ifndef CUINT8_T
# ifdef UINT8_T
typedef struct {
uint8_T re, im;
} cuint8_T;
# define CUINT8_T cuint8_T
# endif
#endif
#ifndef CINT16_T
# ifdef INT16_T
typedef struct {
int16_T re, im;
} cint16_T;
# define CINT16_T cint16_T
# endif
#endif
#ifndef CUINT16_T
# ifdef UINT16_T
typedef struct {
uint16_T re, im;
} cuint16_T;
# define CUINT16_T cuint16_T
# endif
#endif
#ifndef CINT32_T
# ifdef INT32_T
typedef struct {
int32_T re, im;
} cint32_T;
# define CINT32_T cint32_T
# endif
#endif
#ifndef CUINT32_T
# ifdef UINT32_T
typedef struct {
uint32_T re, im;
} cuint32_T;
# define CUINT32_T cuint32_T
# endif
#endif
/*=======================================================================*
* Min and Max: *
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
*=======================================================================*/
#define MAX_int8_T ((int8_T)(127)) /* 127 */
#define MIN_int8_T ((int8_T)(-128)) /* -128 */
#define MAX_uint8_T ((uint8_T)(255)) /* 255 */
#define MIN_uint8_T ((uint8_T)(0))
#define MAX_int16_T ((int16_T)(32767)) /* 32767 */
#define MIN_int16_T ((int16_T)(-32768)) /* -32768 */
#define MAX_uint16_T ((uint16_T)(65535)) /* 65535 */
#define MIN_uint16_T ((uint16_T)(0))
#define MAX_int32_T ((int32_T)(2147483647)) /* 2147483647 */
#define MIN_int32_T ((int32_T)(-2147483647-1)) /* -2147483648 */
#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) /* 4294967295 */
#define MIN_uint32_T ((uint32_T)(0))
#if defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
|| (defined(__WATCOMC__) && __WATCOMC__ >= 1100)
# ifdef INT64_T
# define MAX_int64_T ((int64_T)(9223372036854775807))
# define MIN_int64_T ((int64_T)(-9223372036854775807-1))
# endif
# ifdef UINT64_T
# define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFU))
# define MIN_uint64_T ((uint64_T)(0))
# endif
#else
# ifdef INT64_T
# define MAX_int64_T ((int64_T)(9223372036854775807LL))
# define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL))
# endif
# ifdef UINT64_T
# define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFLLU))
# define MIN_uint64_T ((uint64_T)(0))
# endif
#endif
#ifdef _MSC_VER
/* Conversion from unsigned __int64 to double is not implemented in windows
* and results in a compile error, thus the value must first be cast to
* signed __int64, and then to double.
*
* If the 64 bit int value is greater than 2^63-1, which is the signed int64 max,
* the macro below provides a workaround for casting a uint64 value to a double
* in windows.
*/
# define uint64_to_double(u) ( ((u) > _I64_MAX) ? \
(double)(__int64)((u) - _I64_MAX - 1) + (double)_I64_MAX + 1: \
(double)(__int64)(u) )
/* The largest double value that can be cast to uint64 in windows is the
* signed int64 max, which is 2^63-1. The macro below provides
* a workaround for casting large double values to uint64 in windows.
*/
# define double_to_uint64(d) ( ((d) > 0xffffffffffffffffu) ? \
(unsigned __int64) 0xffffffffffffffffu : \
((d) < 0) ? (unsigned __int64) 0 : \
((d) > _I64_MAX) ? \
(unsigned __int64) ((d) - _I64_MAX) - 1 + (unsigned __int64)_I64_MAX + 1: \
(unsigned __int64)(d) )
#else
# define uint64_to_double(u) ((double)(u))
# if defined(__BORLANDC__) || defined(__WATCOMC__) || defined(__TICCSC__)
/* double_to_uint64 defined only for MSVC and UNIX */
# else
# define double_to_uint64(d) ( ((d) > 0xffffffffffffffffLLU) ? \
(unsigned long long) 0xffffffffffffffffLLU : \
((d) < 0) ? (unsigned long long) 0 : (unsigned long long)(d) )
# endif
#endif
#if !defined(__cplusplus) && !defined(__bool_true_false_are_defined)
#ifndef _bool_T
#define _bool_T
typedef boolean_T bool;
#ifndef false
#define false (0)
#endif
#ifndef true
#define true (1)
#endif
#endif /* _bool_T */
#endif /* !__cplusplus */
/*
* This software assumes that the code is being compiled on a target using a
* 2's complement representation for signed integer values.
*/
#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
#error "This code must be compiled using a 2's complement representation for signed integer values"
#endif
/*
* Maximum length of a MATLAB identifier (function/variable/model)
* including the null-termination character.
*/
#define TMW_NAME_LENGTH_MAX 64
/*
* Maximum values for indices and dimensions
*/
#include <stddef.h>
#ifdef MX_COMPAT_32
typedef int mwSize;
typedef int mwIndex;
typedef int mwSignedIndex;
#else
typedef size_t mwSize; /* unsigned pointer-width integer */
typedef size_t mwIndex; /* unsigned pointer-width integer */
typedef ptrdiff_t mwSignedIndex; /* a signed pointer-width integer */
#endif
#if (defined(_LP64) || defined(_WIN64)) && !defined(MX_COMPAT_32)
/* Currently 2^48 based on hardware limitations */
# define MWSIZE_MAX 281474976710655UL
# define MWINDEX_MAX 281474976710655UL
# define MWSINDEX_MAX 281474976710655L
# define MWSINDEX_MIN -281474976710655L
#else
# define MWSIZE_MAX 2147483647UL
# define MWINDEX_MAX 2147483647UL
# define MWSINDEX_MAX 2147483647L
# define MWSINDEX_MIN -2147483647L
#endif
#define MWSIZE_MIN 0UL
#define MWINDEX_MIN 0UL
#endif /* __TMWTYPES__ */
#endif /* tmwtypes_h */

85
Classes/usb.cpp Normal file
View File

@@ -0,0 +1,85 @@
/*
* usb.cpp
*
* This module has the wrapper functions to access USB driver functions.
*
* Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/
* ALL RIGHTS RESERVED
*
*/
//#ifdef Q_OS_WIN32
//#include <setupapi.h>
//#endif
//#include <QMessageBox>
//#include <QTimer>
#include "usb.h"
//#include <stdio.h>
//#include <stdlib.h>
#include "hidapi.h"
/***************************************************
* GLOBAL VARIABLES
****************************************************/
static hid_device *DeviceHandle; //Handle to write
//In/Out buffers equal to HID endpoint size + 1
//First byte is for Windows internal use and it is always 0
unsigned char OutputBuffer[USB_MAX_PACKET_SIZE+1];
unsigned char InputBuffer[USB_MAX_PACKET_SIZE+1];
static bool USBConnected = false; //Boolean true when device is connected
bool USB_IsConnected()
{
return USBConnected;
}
int USB_Init(void)
{
return hid_init();
}
int USB_Exit(void)
{
return hid_exit();
}
int USB_Open()
{
// Open the device using the VID, PID,
// and optionally the Serial number.
DeviceHandle = hid_open(MY_VID, MY_PID, NULL);
if(DeviceHandle == NULL)
{
USBConnected = false;
return -1;
}
USBConnected = true;
return 0;
}
int USB_Write()
{
if(DeviceHandle == NULL)
return -1;
return hid_write(DeviceHandle, OutputBuffer, USB_MIN_PACKET_SIZE+1);
}
int USB_Read()
{
if(DeviceHandle == NULL)
return -1;
return hid_read_timeout(DeviceHandle, InputBuffer, USB_MIN_PACKET_SIZE+1, 2000);
}
int USB_Close()
{
hid_close(DeviceHandle);
USBConnected = false;
return 0;
}

28
Classes/usb.h Normal file
View File

@@ -0,0 +1,28 @@
/*
* usb.h
*
* This module has the wrapper functions to access USB driver functions.
*
* Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/
* ALL RIGHTS RESERVED
*
*/
#ifndef USB_H
#define USB_H
#define USB_MIN_PACKET_SIZE 64
#define USB_MAX_PACKET_SIZE 64
#define MY_VID 0x0451
#define MY_PID 0x6401
int USB_Open(void);
bool USB_IsConnected();
int USB_Write();
int USB_Read();
int USB_Close();
int USB_Init();
int USB_Exit();
#endif //USB_H

142
Classes/wavCFandSD.cpp Normal file
View File

@@ -0,0 +1,142 @@
//
// File: wavCFandSD.cpp
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
// Include Files
#include "wavCFandSD.h"
#include "cwt.h"
#include "cwt_data.h"
#include "gammaln.h"
#include "rt_nonfinite.h"
#include <cmath>
#include <string.h>
// Function Definitions
//
// Arguments : const char wname[5]
// double varargin_1
// double varargin_2
// double *FourierFactor
// double *sigmaT
// double *cf
// Return Type : void
//
void wavCFandSD(const char wname[5], double varargin_1, double varargin_2,
double *FourierFactor, double *sigmaT, double *cf)
{
char switch_expression;
boolean_T b_bool;
int b_index;
double cf_tmp;
double b_cf_tmp;
double d;
double d1;
double be_tmp;
double be;
double b_be_tmp;
double c_be_tmp;
double b_be;
double d_be_tmp;
double e_be_tmp;
double c_be;
double d2;
double d3;
double d4;
double d5;
double sigmaT_tmp;
double b_sigmaT_tmp;
*cf = 0.0;
*sigmaT = 0.0;
switch_expression = cv1[static_cast<unsigned char>(wname[0]) & 127];
b_bool = !(switch_expression != 'm');
if (b_bool) {
b_index = 0;
} else {
b_bool = !(switch_expression != 'a');
if (b_bool) {
b_index = 1;
} else {
b_bool = !(switch_expression != 'b');
if (b_bool) {
b_index = 2;
} else {
b_index = -1;
}
}
}
switch (b_index) {
case 0:
cf_tmp = std::log(varargin_1);
b_cf_tmp = std::log(varargin_2);
*cf = std::exp(1.0 / varargin_1 * (b_cf_tmp - cf_tmp));
d = 2.0 * varargin_2 + 1.0;
d1 = (d + 2.0) / varargin_1;
gammaln(&d1);
d /= varargin_1;
d1 = d;
gammaln(&d1);
d1 = (2.0 * varargin_2 + 2.0) / varargin_1;
gammaln(&d1);
d1 = d;
gammaln(&d1);
be_tmp = 2.0 * (varargin_2 - 1.0);
be = 2.0 * varargin_2;
b_be_tmp = (varargin_2 - 1.0) + varargin_1;
c_be_tmp = 2.0 * b_be_tmp;
b_be = 2.0 * varargin_2;
d_be_tmp = (varargin_2 - 1.0) + varargin_1 / 2.0;
e_be_tmp = 2.0 * d_be_tmp;
c_be = 2.0 * varargin_2;
d1 = (be_tmp + 1.0) / varargin_1;
gammaln(&d1);
d2 = d;
gammaln(&d2);
d3 = (c_be_tmp + 1.0) / varargin_1;
gammaln(&d3);
d4 = d;
gammaln(&d4);
d5 = (e_be_tmp + 1.0) / varargin_1;
gammaln(&d5);
gammaln(&d);
sigmaT_tmp = varargin_2 / varargin_1;
b_sigmaT_tmp = 2.0 * (sigmaT_tmp * ((cf_tmp + 1.0) - b_cf_tmp));
sigmaT_tmp = 2.0 / varargin_1 * std::log(sigmaT_tmp);
*sigmaT = std::sqrt((std::exp(((((((b_sigmaT_tmp - 2.0 * ((varargin_2 - 1.0)
/ varargin_1 * ((cf_tmp + 1.0) - std::log(varargin_2 - 1.0)))) + be_tmp /
varargin_1 * ((cf_tmp + 1.0) - std::log(be_tmp))) - be / varargin_1 *
((cf_tmp + 1.0) - std::log(be))) + sigmaT_tmp) + 2.0 * b_cf_tmp) + d1) -
d2) + std::exp(((((((b_sigmaT_tmp - 2.0 * (b_be_tmp / varargin_1 *
((cf_tmp + 1.0) - std::log(b_be_tmp)))) + c_be_tmp / varargin_1 * ((cf_tmp
+ 1.0) - std::log(c_be_tmp))) - b_be / varargin_1 * ((cf_tmp + 1.0) - std::
log(b_be))) + sigmaT_tmp) + 2.0 * cf_tmp) + d3) - d4)) - std::exp
(((((((((b_sigmaT_tmp - 2.0 * (d_be_tmp / varargin_1 *
((cf_tmp + 1.0) - std::log(d_be_tmp)))) + e_be_tmp / varargin_1 * ((cf_tmp
+ 1.0) - std::log(e_be_tmp))) - c_be / varargin_1 * ((cf_tmp + 1.0) - std::
log(c_be))) + sigmaT_tmp) + 0.69314718055994529) + b_cf_tmp) + cf_tmp) +
d5) - d));
break;
case 1:
*cf = 6.0;
*sigmaT = 1.4142135623730951;
break;
case 2:
*cf = 5.0;
*sigmaT = 5.847705;
break;
}
*FourierFactor = 6.2831853071795862 / *cf;
}
//
// File trailer for wavCFandSD.cpp
//
// [EOF]
//

27
Classes/wavCFandSD.h Normal file
View File

@@ -0,0 +1,27 @@
//
// File: wavCFandSD.h
//
// MATLAB Coder version : 4.3
// C/C++ source code generated on : 30-Mar-2020 11:59:32
//
#ifndef WAVCFANDSD_H
#define WAVCFANDSD_H
// Include Files
#include <cstddef>
#include <cstdlib>
#include "rtwtypes.h"
#include "omp.h"
#include "cwt_types.h"
// Function Declarations
extern void wavCFandSD(const char wname[5], double varargin_1, double varargin_2,
double *FourierFactor, double *sigmaT, double *cf);
#endif
//
// File trailer for wavCFandSD.h
//
// [EOF]
//

55
UI/Reconstruction.ui
View File

@@ -69,7 +69,7 @@
<string notr="true"/>
</property>
<property name="currentIndex">
<number>2</number>
<number>0</number>
</property>
<widget class="QWidget" name="page">
<widget class="QLabel" name="label_2">
@@ -603,7 +603,7 @@
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
<widget class="QTextBrowser" name="textBrowser_7">
<widget class="QTreeView" name="treeView">
<property name="geometry">
<rect>
<x>10</x>
@@ -612,11 +612,6 @@
<height>181</height>
</rect>
</property>
<property name="font">
<font>
<pointsize>9</pointsize>
</font>
</property>
</widget>
</widget>
</widget>
@@ -1265,6 +1260,15 @@
</color>
</brush>
</colorrole>
<colorrole role="PlaceholderText">
<brush brushstyle="NoBrush">
<color alpha="128">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
</active>
<inactive>
<colorrole role="WindowText">
@@ -1402,6 +1406,15 @@
</color>
</brush>
</colorrole>
<colorrole role="PlaceholderText">
<brush brushstyle="NoBrush">
<color alpha="128">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
</inactive>
<disabled>
<colorrole role="WindowText">
@@ -1539,6 +1552,15 @@
</color>
</brush>
</colorrole>
<colorrole role="PlaceholderText">
<brush brushstyle="NoBrush">
<color alpha="128">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
</disabled>
</palette>
</property>
@@ -1746,5 +1768,22 @@
<resources>
<include location="../Resources/Reconstruction.qrc"/>
</resources>
<connections/>
<connections>
<connection>
<sender>spinBox</sender>
<signal>valueChanged(int)</signal>
<receiver>spinBox</receiver>
<slot>setValue(int)</slot>
<hints>
<hint type="sourcelabel">
<x>1146</x>
<y>98</y>
</hint>
<hint type="destinationlabel">
<x>1146</x>
<y>98</y>
</hint>
</hints>
</connection>
</connections>
</ui>