// Print out kernel parameters.
void GPCMMLPKernel::printParams()
{
DBPRINTLN("MLP variance: " << var(0,0));
DBPRINTLN("MLP weight:");
DBPRINTMAT(wt);
DBPRINTLN("MLP bias:");
DBPRINTMAT(bias);
}
// Callback to compute constraint.
double GPCMOptimization::constraint(
int n, // Dimensionality of problem (should match!).
const double *xdata, // Pointer to new parameter values.
double *grad // Pre-allocated buffer to return data to.
)
{
// Make sure dimensionality matches.
assert(params == n);
// Make sure parameters match.
bool bXChanged = !(Map<VectorXd>((double*)xdata,params,1).array() == x.array()).all();
if (bXChanged)
{ // If parameters changed, must update the model.
DBWARNING("Constraint function called with X vector that differs from last objective call, reevaluating.");
// Copy out the parameter values.
memcpy(x.data(),xdata,sizeof(double)*params);
// Unpack variables.
unpackVariables(x);
// Update the model.
model->recompute(false);
}
// Zero out the gradient.
if (grad)
clearGradients();
// Compute gradients and constraint value.
double constraintValue = model->recomputeConstraint(grad != NULL);
if (grad)
{ // Only do this if grad is not NULL.
// Pack gradients.
packGradients(x,g);
// Copy over the gradients.
memcpy(grad,g.data(),sizeof(double)*params);
// Print message
if (!bSilent)
DBPRINTLN("Constraint gradient evaluation: " << constraintValue);
}
else
{
if (!bSilent)
DBPRINTLN("Constraint value evaluation: " << constraintValue);
}
return constraintValue;
}
void Compass::begin()
{
if (!compass->begin())
{
DBPRINTLN("Oops ... unable to initialize the compass. Check your wiring!");
while (1);
}
DEBUG_PRINT("Compass Setup Complete!");
}
// Run the optimization.
void GPCMOptimization::optimize(
GPCMOptimizable *model // Model to optimize.
)
{
if (maxIterations <= 0) return; // Nothing to optimize.
// Create initial value.
x.resize(params);
g.resize(params);
packVariables(x);
iterations = 0;
// Optionally validate gradients.
if (bValidate)
validateGradients(model);
// Store model.
this->model = model;
// Perform optimization.
int outerIterations = 1;
if (bAlternating)
outerIterations = this->outerIterations; // If alternating, use a number of outer iterations.
for (int itr = 0; itr < outerIterations; itr++)
{
// Run optimization.
if (model->hasConstraint())
algorithm->initialize(params,maxIterations,
objectiveCallback,constraintCallback,this);
else
algorithm->initialize(params,maxIterations,
objectiveCallback,NULL,this);
algorithm->setStart(x);
algorithm->run(x);
// Read back result.
unpackVariables(x);
if (bAlternating)
{ // If doing alternating optimization, perform maximization here.
model->recompute(false);
model->recomputeClosedForm();
packVariables(x);
}
}
clearGradients();
double l = model->recompute(true);
packGradients(x,g);
model->setDebugGradient(g,l);
if (!bSilent)
DBPRINTLN("Final likelihood: " << l);
if (bValidate) validateGradients(model);
return;
}
// Callback to compute objective.
double GPCMOptimization::objective(
int n, // Dimensionality of problem (should match!).
const double *xdata, // Pointer to new parameter values.
double *grad // Pre-allocated buffer to return data to.
)
{
// Make sure dimensionality matches.
assert(params == n);
// Copy out the parameter values.
memcpy(x.data(),xdata,sizeof(double)*params);
// Unpack variables.
unpackVariables(x);
// Zero out the gradient.
if (grad)
clearGradients();
// Compute gradients and objective.
double ll = model->recompute(grad != NULL);
if (grad)
{ // Only do this if grad is not NULL.
// Pack gradients.
packGradients(x,g);
// Copy over the gradients.
memcpy(grad,g.data(),sizeof(double)*params);
// Print iteration.
iterations++;
if (!bSilent)
DBPRINTLN("Evaluation " << iterations << ": " << ll);
}
else if (!bSilent)
{
DBPRINTLN("Linesearch: " << ll);
}
return ll;
}
void GPWithRankPrior::copySettings(const GPWithRankPrior & model)
{
//TODO Need Update One Data Changed
// Make sure number of data points is equal.
assert(model.mX.rows() == mX.rows());
// Copy parameters that don't require special processing.
this->mSequence = model.mSequence;
this->mDataMatrix = model.mDataMatrix;
this->mY = model.mY;
// Copy latent positions or convert them to desired dimensionality.
if (model.mX.cols() == mX.cols())
{
mX = model.mX;
}
else
{
// Pull out the largest singular values to keep.
JacobiSVD<MatrixXd> svd(model.mX, ComputeThinU | ComputeThinV);
VectorXd S = svd.singularValues();
this->mX = svd.matrixU().block(0,0,this->mX.rows(),this->mX.cols())*S.head(this->mX.cols()).asDiagonal();
std::cout << mX << std::endl;
// Report on the singular values that are kept and discarded.
DBPRINTLN("Largest singular value discarded: " << S(this->mX.cols()));
DBPRINTLN("Smallest singular value kept: " << S(this->mX.cols()-1));
DBPRINTLN("Average singular value kept: " << ((1.0/((double)this->mX.cols()))*S.head(this->mX.cols()).sum()));
DBPRINT("Singular values: ");
DBPRINTMAT(S);
}
if (mReconstructionGP) mReconstructionGP->copySettings(model.mReconstructionGP);
if (mBackConstraint) mBackConstraint->copySettings(model.mBackConstraint);
}
GPWithRankPrior::GPWithRankPrior(GPCMOptions &inOptions, bool bLoadTrainedModel, bool bRunHighDimensionalOptimization)
{
GPCMOptions options;
//######### initialization_script
if (!inOptions["model"]["initialization_script"].empty())
{
// Options are replaced with the options of the initialization script.
GPCMScriptParser parser(inOptions["model"]["initialization_script"][0],inOptions["dir"]["dir"][0]);
options = parser.getOptions();
// We necessarily want to load the trained model.
bLoadTrainedModel = true;
options["data"]["initialization_file"] = options["result"]["mat_file"];
options["data"]["initialization_file"][0].insert(0,options["dir"]["dir"][0]);
// Override options.
for (GPCMOptions::iterator itr = inOptions.begin(); itr != inOptions.end(); itr++)
{
for (GPCMParams::iterator pitr = itr->second.begin(); pitr != itr->second.end(); pitr++)
{
options[itr->first][pitr->first] = pitr->second;
}
}
}
else if (bLoadTrainedModel)
{ // Set initialization file if we are loading a trained model.
options = inOptions;
options["data"]["initialization_file"] = options["result"]["mat_file"];
options["data"]["initialization_file"][0].insert(0,options["dir"]["dir"][0]);
}
else
{
options = inOptions;
}
GPWithRankPrior *initModel = NULL;
mbHighDimensionalOptimization = bRunHighDimensionalOptimization;
if (!bRunHighDimensionalOptimization &&
!inOptions["embedding"]["training_latent_dimensions"].empty() &&
options["data"]["initialization_file"].empty())
{
// Launch high-dimensional pre-training.
DBPRINTLN("Launching high dimensional optimization...");
initModel = new GPWithRankPrior(options, false, true);
initModel->optimize();
DBPRINTLN("High dimensional optimization complete.");
}
//create data reader
std::string datatype = options["data"]["type"][0];
std::vector<std::string> data = options["data"]["path"];
GPCMDataReader *datareader = NULL;
if (!datatype.compare("bvh_motion")) // Read BVH motion data.
datareader = new GPCMDataReaderBVH();
else if (!datatype.compare("bvh_motion_quat")) // Read BVH motion data but use quaternion representation.
datareader = new GPCMDataReaderBVHQuat();
else if (!datatype.compare("annotation")) // Read ANN annotation data.
datareader = new GPCMDataReaderANN();
else // Unknown data.
DBERROR("Unknown data type " << datatype << " specified!");
std::vector<double> noise(data.size());
for (unsigned i = 0; i < data.size(); i++)
{
if (options["data"]["noise"].size() > i)
noise[i] = atof(options["data"]["noise"][i].c_str());
else
noise[i] = 0.0;
}
// append absloute path
for (std::vector<std::string>::iterator itr = data.begin(); itr != data.end(); itr++)
{
itr->insert(0,options["dir"]["dir"][0]);
}
datareader->load(data,noise);
mDataMatrix = datareader->getYMatrix();
mSequence = datareader->getSequence();
delete datareader;
bool bValidate = !options["optimization"]["validate_gradients"][0].compare("true");
int maxIterations;
if (!options["optimization"]["iterations_lowdim"].empty() && !bRunHighDimensionalOptimization)
maxIterations = atoi(options["optimization"]["iterations_lowdim"][0].c_str());
else
maxIterations = atoi(options["optimization"]["iterations"][0].c_str());
bool bUseEM = !options["model"]["learn_scales"][0].compare("em");
int outerIterations = 1;
if (bUseEM)
outerIterations = atoi(options["optimization"]["outer_iterations"][0].c_str());
if (!options["data"]["initialization_file"].empty())
mbRunOptimization = false;
else
mbRunOptimization = true;
mOptimization = new GPCMOptimization( bValidate, bUseEM, options["optimization"]["algorithm"][0],maxIterations,false);
mLatDim = 1;
if (bRunHighDimensionalOptimization)
mLatDim = atoi(options["embedding"]["training_latent_dimensions"][0].c_str());
else
mLatDim = atoi(options["embedding"]["latent_dimensions"][0].c_str());
if (mLatDim > mDataMatrix.cols()) mLatDim = mDataMatrix.cols();
mX.resize(mDataMatrix.rows(),mLatDim);
mXGrad.resize(mDataMatrix.rows(),mLatDim);
// std::string inittype = options["initialization"]["method"][0];
// Optionally filter the data matrix.
MatrixXd filteredDataMatrix;
if (!options["initialization"]["prefiltering"].empty())
filteredDataMatrix = filterData(mDataMatrix,mSequence,atof(options["initialization"]["prefiltering"][0].c_str()));
else
filteredDataMatrix = mDataMatrix;
mBackConstraint = GPCMBackConstraint::createBackConstraint(options["back_constraints"],options,
mOptimization,mDataMatrix,mX);
MatrixXd *Xptr = &mX;
MatrixXd *Xgradptr = &mXGrad;
mReconstructionGP = new GPCMGaussianProcess(options["model"],options,
mOptimization,NULL,mDataMatrix,mY,&Xptr,&Xgradptr,1,false,true);
// if(!mbHighDimensionalOptimization)
GPCMEmbedPPCA(mX,mDataMatrix);
// else
// mX = mY;
if(mBackConstraint!= nullptr)
mBackConstraint->initialize();
if (mBackConstraint == nullptr)
mOptimization->addVariable(VarXformNone,&mX,&mXGrad,"X");
if (bRunHighDimensionalOptimization && !options["model"]["rank_prior_wt"].empty())
mRankPrior = new GPCMRankPrior(atof(options["model"]["rank_prior_wt"][0].c_str()),mX,mXGrad);
else
mRankPrior = nullptr;
//----------------------------------------------------------------------------------------------
if (initModel)
{
this->copySettings(*initModel);
delete initModel;
}
else if (!options["data"]["initialization_file"].empty())
{
// If we have an initialization file, load that now.
GPCMMatReader *reader = new GPCMMatReader(options["data"]["initialization_file"][0]);
load(reader->getStruct("model"));
delete reader;
}
recompute(true);
}
void aci_loop(int *flag)
{
// We enter the if statement only when there is a ACI event available to be processed
if (lib_aci_event_get(&aci_state, &aci_data))
{
aci_evt_t * aci_evt;
DBPRINTLN("There is an ACI Events available");
aci_evt = &aci_data.evt;
switch(aci_evt->evt_opcode)
{
/* As soon as you reset the nRF8001 you will get an ACI Device Started Event */
case ACI_EVT_DEVICE_STARTED:
{
aci_state.data_credit_total = aci_evt->params.device_started.credit_available;
switch(aci_evt->params.device_started.device_mode)
{
/** When the device is in the setup mode */
case ACI_DEVICE_SETUP:
DBPRINTLN("Evt Device Started: Setup");
if (ACI_STATUS_TRANSACTION_COMPLETE != do_aci_setup(&aci_state))
DBPRINTLN("Error in ACI Setup");
break;
case ACI_DEVICE_STANDBY:
DBPRINTLN("Evt Device Started: Standby");
//Looking for an iPhone by sending radio advertisements
//When an iPhone connects to us we will get an ACI_EVT_CONNECTED event from the nRF8001
lib_aci_connect(180/* in seconds */, 0x0050 /* advertising interval 50ms*/);
DBPRINTLN("Advertising started");
break;
}
}
break; //ACI Device Started Event
case ACI_EVT_CMD_RSP:
//If an ACI command response event comes with an error -> stop
if (ACI_STATUS_TRANSACTION_CONTINUE == aci_evt->params.cmd_rsp.cmd_status) {
DBPRINTLN("Reading/Writing dynamic data...");
}
else if (ACI_STATUS_TRANSACTION_COMPLETE == aci_evt->params.cmd_rsp.cmd_status) {
DBPRINTLN("Reading/Writing dynamic data finished.");
}
else if (ACI_STATUS_SUCCESS != aci_evt->params.cmd_rsp.cmd_status)
{
//ACI ReadDynamicData and ACI WriteDynamicData will have status codes of
//TRANSACTION_CONTINUE and TRANSACTION_COMPLETE
//all other ACI commands will have status code of ACI_STATUS_SCUCCESS for a successful command
DBPRINT("Error ACI Command ");
Serial.print(aci_evt->params.cmd_rsp.cmd_opcode, HEX);
DBPRINT(" Evt Cmd respone error code: ");
Serial.println(aci_evt->params.cmd_rsp.cmd_status, HEX);
//while (1);
}
// react to different command responses
switch (aci_evt->params.cmd_rsp.cmd_opcode) {
case ACI_CMD_GET_DEVICE_VERSION:
// Debug print
DBPRINTLN("Debug: printting configuration id, aci version, setup format, id, and status from the cmd rsp opcode:");
DBPRINTLN(aci_evt->params.cmd_rsp.params.get_device_version.configuration_id);
DBPRINTLN(aci_evt->params.cmd_rsp.params.get_device_version.aci_version);
DBPRINTLN(aci_evt->params.cmd_rsp.params.get_device_version.setup_format);
DBPRINTLN(aci_evt->params.cmd_rsp.params.get_device_version.setup_id);
DBPRINTLN(aci_evt->params.cmd_rsp.params.get_device_version.setup_status);
//Store the version and configuration information of the nRF8001 in the Hardware Revision String Characteristic
lib_aci_set_local_data(&aci_state, PIPE_DEVICE_INFORMATION_HARDWARE_REVISION_STRING_SET,
(uint8_t *)&(aci_evt->params.cmd_rsp.params.get_device_version), sizeof(aci_evt_cmd_rsp_params_get_device_version_t));
break;
case ACI_CMD_GET_DEVICE_ADDRESS:
// Debug print
DBPRINTLN("Debug: printting device address, and address type:");
DBPRINTLN(aci_evt->params.cmd_rsp.params.get_device_address.bd_addr_own[0]);
DBPRINTLN(aci_evt->params.cmd_rsp.params.get_device_address.bd_addr_type);
break;
case ACI_CMD_GET_TEMPERATURE:
// Debug print
DBPRINTLN("Debug: printting device temperature:");
DBPRINTLN(aci_evt->params.cmd_rsp.params.get_temperature.temperature_value/4);
break;
case ACI_CMD_READ_DYNAMIC_DATA:
// Debug print
DBPRINT("Debug: dynamic data:");
DBPRINTLN(aci_evt->params.cmd_rsp.params.get_temperature.temperature_value/4);
break;
}
break;
case ACI_EVT_CONNECTED:
DBPRINTLN("Evt Connected");
aci_state.data_credit_available = aci_state.data_credit_total;
// Get the device version of the nRF8001 and store it in the Hardware Revision String
lib_aci_device_version();
break;
case ACI_EVT_PIPE_STATUS:
DBPRINTLN("Evt Pipe Status");
if (lib_aci_is_pipe_available(&aci_state, PIPE_UART_OVER_BTLE_UART_TX_TX) && (false == timing_change_done))
{
lib_aci_change_timing_GAP_PPCP(); // change the timing on the link as specified in the nRFgo studio -> nRF8001 conf. -> GAP.
// Used to increase or decrease bandwidth
timing_change_done = true;
}
if (lib_aci_is_pipe_available(&aci_state, PIPE_UART_OVER_BTLE_UART_TX_TX)) {
DBPRINTLN("UART pipe over BLE is available");
*flag = 1;
}
break;
case ACI_EVT_TIMING:
DBPRINTLN("Evt link connection interval changed");
break;
case ACI_EVT_DISCONNECTED:
DBPRINTLN("Evt Disconnected/Advertising timed out");
lib_aci_connect(180/* in seconds */, 0x0100 /* advertising interval 100ms*/);
DBPRINTLN("Advertising started");
*flag = 0;
break;
case ACI_EVT_DATA_RECEIVED:
DBPRINT("UART RX: 0x");
Serial.print(aci_evt->params.data_received.rx_data.pipe_number, HEX);
{
DBPRINT(" Data(Hex) : ");
for(int i=0; i<aci_evt->len - 2; i++)
{
Serial.print(aci_evt->params.data_received.rx_data.aci_data[i], HEX);
uart_buffer[i] = aci_evt->params.data_received.rx_data.aci_data[i];
DBPRINT(" ");
}
uart_buffer_len = aci_evt->len - 2;
}
DBPRINT("I got the request");
break;
case ACI_EVT_DATA_CREDIT:
aci_state.data_credit_available = aci_state.data_credit_available + aci_evt->params.data_credit.credit;
break;
case ACI_EVT_PIPE_ERROR:
//See the appendix in the nRF8001 Product Specication for details on the error codes
DBPRINT("ACI Evt Pipe Error: Pipe #:");
DBPRINT(aci_evt->params.pipe_error.pipe_number);
DBPRINT(" Pipe Error Code: 0x");
DBPRINTLN(aci_evt->params.pipe_error.error_code);
//Increment the credit available as the data packet was not sent
aci_state.data_credit_available++;
break;
default:
DBPRINTLN("Unrecognized Event");
break;
}
}
else
{
//DBPRINTLN(F("No ACI Events available"));
// No event in the ACI Event queue and if there is no event in the ACI command queue the arduino can go to sleep
// Arduino can go to sleep now
// Wakeup from sleep from the RDYN line
}
}
void print_aci(){
DBPRINT("Pipe type mapping location:");
DBPRINTLN(aci_state.aci_setup_info.services_pipe_type_mapping->location);
DBPRINT("Pipe type mapping pipe type:");
DBPRINTLN(aci_state.aci_setup_info.services_pipe_type_mapping->pipe_type);
DBPRINT("bonded?: ");DBPRINTLN(aci_state.bonded); /* ( aci_bond_status_code_t ) Is the nRF8001 bonded to a peer device */
DBPRINT("totol credit: ");DBPRINTLN(aci_state.data_credit_total); /* Total data credit available for the specific version of the nRF8001, total equals available when a link is established */
DBPRINT("device status: ");DBPRINTLN(aci_state.device_state); /* Operating mode of the nRF8001 */
/* Start : Variables that are valid only when in a connection */
DBPRINT("available credits: ");DBPRINTLN(aci_state.data_credit_available); /* Available data credits at a specific point of time, ACI_EVT_DATA_CREDIT updates the available credits */
DBPRINT("connection interval: ");DBPRINTLN(aci_state.connection_interval); /* Multiply by 1.25 to get the connection interval in milliseconds*/
DBPRINT("slave latency: ");DBPRINTLN(aci_state.slave_latency); /* Number of consecutive connection intervals that the nRF8001 is not required to transmit. Use this to save power */
DBPRINT("supervision timeout: ");DBPRINTLN(aci_state.supervision_timeout); /* Multiply by 10 to get the supervision timeout in milliseconds */
DBPRINT("pips open bitmap: ");DBPRINTLN(aci_state.pipes_open_bitmap[PIPES_ARRAY_SIZE]); /* Bitmap -> pipes are open and can be used for sending data over the air */
DBPRINT("pipes closed bitmap: ");DBPRINTLN(aci_state.pipes_closed_bitmap[PIPES_ARRAY_SIZE]); /* Bitmap -> pipes are closed and cannot be used for sending data over the air */
DBPRINT("confirmation pending: ");DBPRINTLN(aci_state.confirmation_pending); /* Attribute protocol Handle Value confirmation is pending for a Handle Value Indication
(ACK is pending for a TX_ACK pipe) on local GATT Server*/
/* End : Variables that are valid only when in a connection */
}
// Print out kernel parameters.
void GPCMWhiteKernel::printParams()
{
DBPRINTLN("White noise variance: " << var(0,0));
}
// Validate gradients with finite differences.
void GPCMOptimization::validateGradients(
GPCMOptimizable *model // Model to validate gradients for.
)
{
// Compute gradient.
clearGradients();
double center = model->recompute(true);
double centerc = 0.0;
packGradients(x,g);
// std::cout << x << std::endl;
// Optionally compute the constraint gradient.
VectorXd cg(g.rows());
if (model->hasConstraint())
{
clearGradients();
centerc = model->recomputeConstraint(true);
packGradients(x,cg);
}
// Take samples to evaluate finite differences.
VectorXd pt = x;
VectorXd fdg(params);
VectorXd fdgc(params);
for (int i = 0; i < params; i++)
{
// Evaluate upper and lower values.
pt.noalias() = x + VectorXd::Unit(params,i)*FD_EPSILON;
unpackVariables(pt);
double valp = model->recompute(false);
double valpc = model->recomputeConstraint(false);
pt.noalias() = x - VectorXd::Unit(params,i)*FD_EPSILON;
unpackVariables(pt);
double valm = model->recompute(false);
double valmc = model->recomputeConstraint(false);
fdg(i) = 0.5*(valp-valm)/FD_EPSILON;
fdgc(i) = 0.5*(valpc-valmc)/FD_EPSILON;
DBPRINTLN("Computed finite difference for dimension " << i << " of " << params << ": " << fdg(i));
}
// std::cout << x << std::endl;
// Reset variables.
unpackVariables(x);
// Construct gradient names.
std::vector<std::string> varname(x.rows());
for (std::vector<GPCMOptVariable>::iterator itr = variables.begin();
itr != variables.end(); itr++)
{
for (int i = itr->getIndex(); i < itr->getIndex()+itr->getParamCount(); i++)
{
varname[i] = itr->getName();
if (itr->getParamCount() > 1)
varname[i] += std::string(" ") +
boost::lexical_cast<std::string>(i-itr->getIndex());
}
}
// Print gradients.
int idx;
DBPRINTLN("True gradient / finite-difference gradient:");
for (int i = 0; i < params; i++)
{
if (model->hasConstraint())
{
DBPRINTLN(std::setw(10) << g(i) << " " <<
std::setw(10) << fdg(i) <<
std::setw(10) << cg(i) << " " <<
std::setw(10) << fdgc(i) <<
std::setw(10) << "(" << x(i) << ")" << " " << varname[i]);
}
else
{
DBPRINTLN(std::setw(10) << g(i) << " " <<
std::setw(10) << fdg(i) <<
std::setw(10) << "(" << x(i) << ")" << " " << varname[i]);
}
}
// Check objective gradient.
double maxDiff = (g-fdg).array().abs().matrix().maxCoeff(&idx);
if (maxDiff >= 0.1)
DBWARNING("Gradients appear significantly different!");
DBPRINTLN("Max difference: " << maxDiff);
DBPRINTLN("Max difference at index " << idx << ":" << std::endl << std::setw(10) << g(idx)
<< " " << std::setw(10) << fdg(idx) << " " << varname[idx]);
if (model->hasConstraint())
{
// Check constraint gradient.
maxDiff = (cg-fdgc).array().abs().matrix().maxCoeff(&idx);
if (maxDiff >= 0.1)
DBWARNING("Constraint gradients appear significantly different!");
DBPRINTLN("Max constraint difference: " << maxDiff);
DBPRINTLN("Max constraint difference at index " << idx << ":" << std::endl << std::setw(10) << cg(idx)
<< " " << std::setw(10) << fdgc(idx) << " " << varname[idx]);
}
}
Compass::Compass(WorldState* ms, float minMagCal[], float maxMagCal[])
{
myState = ms;
compass = new Adafruit_LSM303();
calMin.x = minMagCal[0];
calMin.y = minMagCal[1];
calMin.z = minMagCal[2];
calMax.x = maxMagCal[0];
calMax.y = maxMagCal[1];
calMax.z = maxMagCal[2];
DBPRINT("Compass cal min: ");DBPRINT(calMin.x);DBPRINT(",");DBPRINT(calMin.y);DBPRINT(",");DBPRINT(calMin.z);
DBPRINT("\n max: ");DBPRINT(calMax.x);DBPRINT(",");DBPRINT(calMax.y);DBPRINT(",");DBPRINTLN(calMax.z);
}