ModelPtr GetModel(const std::string& modelName)
{
std::string name = boost::to_lower_copy(modelName) + ".mod";
auto fileIter = m_modelsByFile.find(name);
if (fileIter != m_modelsByFile.end())
{
return fileIter->second;
}
auto iter = m_models.find(modelName);
if (iter != m_models.end())
{
return iter->second;
}
EQEmu::EQGModelLoader model_loader;
ModelPtr model;
if (model_loader.Load(m_archive, name, model))
{
model->SetName(modelName);
m_models[modelName] = model;
}
return model;
}
void surf_presolve(void)
{
double next_event_date = -1.0;
tmgr_trace_event_t event = NULL;
double value = -1.0;
ResourcePtr resource = NULL;
ModelPtr model = NULL;
unsigned int iter;
XBT_DEBUG
("First Run! Let's \"purge\" events and put models in the right state");
while ((next_event_date = tmgr_history_next_date(history)) != -1.0) {
if (next_event_date > NOW)
break;
while ((event =
tmgr_history_get_next_event_leq(history, next_event_date,
&value,
(void **) &resource))) {
if (value >= 0){
resource->updateState(event, value, NOW);
}
}
}
xbt_dynar_foreach(model_list, iter, model)
model->updateActionsState(NOW, 0.0);
}
void Optimizer::TwoPassZbuffer(const CameraParameter& rho, const IlluminationParameter& lamda, MeshPtr mesh, ModelPtr model, bool segment)
{
int width = model->Rows();
int height = model->Cols();
mesh->UpdateVertexNormal();
shared_ptr<Camera> camera = make_shared<PinholeCamera>(rho, width, height);
shared_ptr<Camera> light = make_shared<PinholeCamera>(rho, lamda, width, height);
shared_ptr<Illumination> illumination = make_shared<PhongIllumination>(lamda);
HardwareRender render(camera, illumination);
vector<float> depth_map(width*height);
render.Rendering(mesh, lamda, depth_map);
HardwareRender render2(light, illumination);
vector<float> shadow_map(width*height);
render2.Rendering(mesh, lamda, shadow_map);
shared_ptr<DepthBuffer> object_depth = nullptr;
shared_ptr<BoxRaster> raster = nullptr;
SoftwareRender object_render(camera, illumination, object_depth, raster);
if (segment)
{
object_render.TwoPassZbufferSegment(light, mesh, model, depth_map, shadow_map);
}
else
{
object_render.TwoPassZbuffer(light, mesh, model, depth_map, shadow_map);
}
}
void EditorChunkFlare::draw()
{
if (!edShouldDraw())
return;
ModelPtr model = reprModel();
if( WorldManager::instance().drawSelection() && model)
{
// draw a some points near the centre of the reprModel, so the system
// can be selected from the distance where the repr model might be
// smaller than a pixel and fail to draw.
Moo::rc().push();
Moo::rc().world( chunk()->transform() );
Moo::rc().preMultiply( edTransform() );
// bias of half the size of the representation model's bounding box in
// the vertical axis, because the object might be snapped to terrain
// or another object, so the centre might be below something else.
float bias = model->boundingBox().width() / 2.0f;
Vector3 points[3];
points[0] = Vector3( 0.0f, -bias, 0.0f );
points[1] = Vector3( 0.0f, 0.0f, 0.0f );
points[2] = Vector3( 0.0f, bias, 0.0f );
Geometrics::drawPoints( points, 3, 3.0f, (DWORD)this );
Moo::rc().pop();
}
EditorChunkSubstance<ChunkFlare>::draw();
}
void ThrusterTask::setGazeboModel( ModelPtr model )
{
string taskName = "gazebo:" + model->GetWorld()->GetName() + ":" + model->GetName() + ":gazebo_thruster";
provides()->setName(taskName);
_name.set(taskName);
topicName = model->GetName() + "/thrusters";
}
void Optimizer::ShowPoints(mat& alpha,
mat& beta,
mat& rho,
mat& lamda,
InputPtr input,
ModelPtr model,
MeshPtr mesh,
ShapePtr shape,
TexturePtr texture,
int point_type)
{
mat alpha_gradient(PrincipalNum, 1);
alpha_gradient.fill(0);
mat beta_gradient(PrincipalNum, 1);
beta_gradient.fill(0);
mat rho_gradient(RhoNum, 1);
rho_gradient.fill(0);
mat lamda_gradient(LamdaNum, 1);
lamda_gradient.fill(0);
Alpha alpha_para(alpha, input, model, mesh, shape, texture);
Beta beta_para(beta, model, mesh, shape, texture);
Rho rho_para(rho, input, model, mesh, shape, texture);
Lamda lamda_para(lamda, model, mesh, shape, texture);
switch (point_type)
{
case RANDOM:
model->InitialRandomGenerator();
GenerateRandomPoints(model, 40);
ShowRandomPoints(input, model, &alpha_para);
break;
case VISIBLE_TRIANGLE:
model->EnableIterator();
ShowVisiblePoints(input, model, &alpha_para);
break;
case SHADOW_TRIANGLE:
model->EnableIterator();
ShowShadowPoints(input, model, &alpha_para);
break;
case SEGMENT:
ShowSegmentPoints(input, model, &alpha_para);
break;
default:
break;
}
model->Show();
}
void add(ModelPtr model)
{
auto it = models.at(model->get_id());
if (it == models.end())
{
models.emplace(model->get_id(), model);
added.trigger(*model);
}
}
virtual bool Load() override
{
bool loadedSomething = m_archive.Open(GetZoneFile(m_zd));
std::string base_filename = (boost::format("%s\\%s")
% m_zd->GetEQPath()
% m_zd->GetZoneName()).str();
// next we need to try to read an _assets file and load more eqg based data.
std::string assets_file = base_filename + "_assets.txt";
std::error_code ec;
if (sys::exists(assets_file, ec))
{
std::vector<std::string> filenames;
std::ifstream assets(assets_file.c_str());
if (assets.is_open())
{
std::copy(std::istream_iterator<std::string>(assets),
std::istream_iterator<std::string>(),
std::back_inserter(filenames));
for (auto& name : filenames)
{
std::string asset_file = (boost::format("%s\\%s") % m_zd->GetEQPath() % name).str();
EQEmu::PFS::Archive archive;
if (!archive.Open(asset_file))
continue;
std::vector<std::string> models;
if (archive.GetFilenames("mod", models))
{
for (auto& modelName : models)
{
EQEmu::EQGModelLoader model_loader;
ModelPtr model;
model_loader.Load(archive, modelName, model);
if (model)
{
model->SetName(modelName);
m_modelsByFile[modelName] = model;
loadedSomething = true;
}
}
}
}
}
}
return loadedSomething;
}
//load textures to OpenGL
static void loadTextures(){
ImagePtr img;
BOOST_FOREACH(const MaterialPtr material, gMaterials){
//texture
if(img = gModel->getImage(material->mTexture)) gTextures[material->mTexture] = loadTexture(img);
//bump
if(img = gModel->getImage(material->mBump)) gTextures[material->mBump] = loadTexture(img);
//reflection map
if(img = gModel->getImage(material->mRefl)) gTextures[material->mRefl] = loadTexture(img);
}
}
void Optimizer::ShowVisiblePoints(InputPtr input, ModelPtr model, Parameter* para) const
{
int num = model->Size();
for (int i = 0; i<num;++i)
{
ivec2 pos = para->ComputePosition(i);
vec3 rgb = para->ComputeColor(i);
//vec3 rgb = input_image->GetColor(Point(pos[0], pos[1]));
//Vec3b color(static_cast<uchar>(rgb[2]), static_cast<uchar>(rgb[1]), static_cast<uchar>(rgb[0]));
Vec3b color(0, 0, 255);
model->WriteColor(pos[0], pos[1], color);
}
cout << "visible triangle number = " << num << endl;
}
void Optimizer::CpuRendering(const CameraParameter& rho,
const IlluminationParameter& lamda,
MeshPtr mesh,
ModelPtr model)
{
int width = model->Rows();
int height = model->Cols();
shared_ptr<Camera> camera = make_shared<PinholeCamera>(rho, width, height);
shared_ptr<Illumination> illumination = make_shared<PhongIllumination>(lamda);
shared_ptr<DepthBuffer> depth = make_shared<DepthBuffer>(width, height);
shared_ptr<BoxRaster> raster = make_shared<BoxRaster>();
SoftwareRender cpu_render(camera, illumination, depth, raster);
cpu_render.Rending(mesh, model);
}
void Optimizer::FitRest(mat& alpha,
mat& beta,
mat& rho,
mat& lamda,
InputPtr input,
ModelPtr model,
MeshPtr mesh,
ShapePtr shape,
TexturePtr texture)
{
mat alpha_gradient = mat(PrincipalNum, 1, fill::zeros);
mat alpha_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
mat beta_gradient = mat(PrincipalNum, 1, fill::zeros);
mat beta_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
Rho rho_para(rho, input, model, mesh, shape, texture);
Lamda lamda_para(lamda, model, mesh, shape, texture);
// 2500 1000 700 500 300 200
model->InitialRandomGenerator(ModelImage::REST);
double weight = 1.0 / 200;
for (int l = 0; l < 1000; ++l)
{
Alpha alpha_para(alpha, input, model, mesh, shape, texture);
Beta beta_para(beta, model, mesh, shape, texture);
// Generate random points
GenerateRandomPoints(model, GradientRandomNum);
double function_value = 0;
for (int i = 0; i < PrincipalNum; ++i)
{
double variance = shape->GetVariance(i);
alpha_gradient[i] = weight * ComputeIntensityGradient(input, &alpha_para, i) + 2 * alpha[i] / variance;
}
for (int i = 0; i < PrincipalNum; ++i)
{
double variance = texture->GetVariance(i);
beta_gradient[i] = weight * ComputeIntensityGradient(input, &beta_para, i) + 2 * beta[i] / variance;
}
alpha -= alpha_para.Step*alpha_gradient;
beta -= beta_para.Step*beta_gradient;
}
}
ModelHandler ResourceManager::GetAnimatedModelFromFile(std::string fileName,std::string resID)
{
unsigned int id = hasher(resID);
if(pool->ModelExist(id))
{
return ModelHandler(pool->GetModel(id),id,pool);
}
else
{
ModelPtr modelPtr = AssetLoader::GetPtr()->LoadAnimatedModel(fileName);
if(modelPtr.IsNull())
return ModelHandler(false);
pool->AddModel(id,modelPtr);
return ModelHandler(modelPtr,id,pool);
}
}
void Optimizer::GpuRendering(const CameraParameter& rho,
const IlluminationParameter& lamda,
MeshPtr mesh,
ModelPtr model)
{
int width = model->Rows();
int height = model->Cols();
mesh->UpdateVertexNormal();
shared_ptr<Camera> camera = make_shared<PinholeCamera>(rho, width, height);
shared_ptr<Camera> light = make_shared<PinholeCamera>(rho, lamda, ShadowResolution, ShadowResolution);
shared_ptr<Illumination> illumination = make_shared<PhongIllumination>(lamda);
//HardwareRender gpu_render(camera, light,illumination);
HardwareRender gpu_render(camera, illumination);
gpu_render.RenderingWithBackground(mesh, model, lamda);
//gpu_render.RenderingWithShadow(mesh, lamda);
}
void remove(ModelPtr model)
{
auto it = models.at(model->get_id());
if (it != models.end())
{
models.erase(it);
removed.trigger(*model);
}
}
void Optimizer::ShowShadowPoints(InputPtr input, ModelPtr model, Parameter* para) const
{
int num = model->Size();
int shadow_counter = 0;
for (int i = 0; i< num; ++i)
{
if (model->visible_triangles_[i].cast_shadow)
{
++shadow_counter;
ivec2 pos = para->ComputePosition(i);
vec3 rgb = para->ComputeColor(i);
//vec3 rgb = input_image->GetColor(Point(pos[0], pos[1]));
// Vec3b color(static_cast<uchar>(rgb[2]), static_cast<uchar>(rgb[1]), static_cast<uchar>(rgb[0]));
Vec3b color(0, 0, 255);
model->WriteColor(pos[0], pos[1], color);
}
}
cout << "shadow triangle number = " << shadow_counter << endl;
}
void updateWidgets(){
const ModelPtr m = controller.getModel();
hscaleMotionThreshold->set_value((double)m->getMotionThreshold());
hscaleSecsBtwAlarm->set_value((double)controller.getSecsBtwAlarm());
const NullAlgorithm *nAlg;
const OpticalFlowAlgorithm *ofAlg;
const PixelDifferenceAlgorithm *pdAlg;
if ((nAlg = dynamic_cast<const NullAlgorithm*>(controller.getModel()->getMotionDetectionAlgorithm().get()))){
comboboxAlgorithm->set_active(0);
}else if ((ofAlg = dynamic_cast<const OpticalFlowAlgorithm*>(controller.getModel()->getMotionDetectionAlgorithm().get()))){
hscaleOpticalFlowNPoints->set_value(ofAlg->nPoints);
hscaleOpticalFlowThreshold->set_value(ofAlg->opticalFlowThreshold);
comboboxAlgorithm->set_active(1);
}else if ((pdAlg = dynamic_cast<const PixelDifferenceAlgorithm*>(controller.getModel()->getMotionDetectionAlgorithm().get()))){
hscalePixelDifferenceThreshold->set_value(pdAlg->pixelDiffThreshold);
hscalePixelDifferenceXStep->set_value(pdAlg->winSize.width);
hscalePixelDifferenceYStep->set_value(pdAlg->winSize.height);
comboboxAlgorithm->set_active(2);
}
}
/**
* This private static method performs operations common to
* both the 'get' and 'reload' methods.
*/
ModelPtr Model::load( const std::string & resourceID, DataSectionPtr pFile )
{
ModelFactory factory( resourceID, pFile );
ModelPtr model = Moo::createModelFromFile( pFile, factory );
if (!model)
{
//ERROR_MSG( "Could not load model: '%s'\n", resourceID.c_str() );
return NULL;
}
PostLoadCallBack::run();
if (!(model && model->valid()))
{
//ERROR_MSG( "Error in post load of model: '%s'\n", resourceID.c_str() );
return NULL;
}
return model;
}
void EditorChunkBinding::draw()
{
if (!edShouldDraw())
return;
if (WorldManager::instance().drawSelection())
{
WorldManager::instance().registerDrawSelectionItem( this );
}
else
{
// draw a line back to the cluster (if applicable)
Moo::rc().push();
Moo::rc().world( Matrix::identity );
const Moo::Colour lineColour = 0xffff0000;
Vector3 from = this->from()->chunk()->transform().applyPoint( this->from()->edTransform().applyToOrigin() )
+ Vector3(0.f, 0.1f, 0.f);
Vector3 to = this->to()->chunk()->transform().applyPoint( this->to()->edTransform().applyToOrigin() )
+ Vector3(0.f, 0.1f, 0.f);
Geometrics::drawLine( from, to, lineColour, false ); // false = z-buffer the lines
Moo::rc().pop();
}
// draw binding item
ModelPtr model = reprModel();
if (model)
{
Moo::rc().push();
Moo::rc().preMultiply( this->edTransform() );
model->dress();
model->draw( true );
Moo::rc().pop();
}
}
void RockBridge::instantiateSensorComponents(sdf::ElementPtr modelElement, ModelPtr model)
{
sdf::ElementPtr linkElement = modelElement->GetElement("link");
while( linkElement ){
sdf::ElementPtr sensorElement = linkElement->GetElement("sensor");
while( sensorElement ){
string sensorName = sensorElement->Get<string>("name");
string sensorType = sensorElement->Get<string>("type");
// To support more sensors, test for different sensors types
if( sensorType == "ray" )
{
gzmsg << "RockBridge: creating laser line component: " + sensorName << endl;
LaserScanTask* laser_line_task = new LaserScanTask();
string topicName = model->GetName() + "/" + linkElement->Get<string>("name") + "/" + sensorName + "/scan";
laser_line_task->setGazeboModel( model, sensorName, topicName );
setupTaskActivity( laser_line_task );
}
else if(sensorType == "camera")
{
gzmsg << "RockBridge: creating camera component: " + sensorName << endl;
CameraTask* camera = new CameraTask();
string topicName = model->GetName() + "/" + linkElement->Get<string>("name") + "/" + sensorName + "/image";
camera->setGazeboModel(model, sensorName, topicName);
setupTaskActivity(camera);
}
else if(sensorType == "imu")
{
gzmsg << "RockBridge: creating imu component: " + sensorName << endl;
ImuTask *imu = new ImuTask();
string topicName = model->GetName() + "/" + linkElement->Get<string>("name") + "/" + sensorName + "/imu";
imu->setGazeboModel(model, sensorName, topicName);
setupTaskActivity(imu);
}
sensorElement = sensorElement->GetNextElement("sensor");
}
linkElement = linkElement->GetNextElement("link");
}
}
void Optimizer::GenerateRandomPoints(ModelPtr model, int num)
{
model->GenerateRandomNumbers(num,random_points_);
//ofstream random;
// random.open("random_points", ios::app);
//for (auto e:random_points_)
//{
// //cout << e << endl;
// random << e << "\n";
//}
//random.close();
}
void Optimizer::ShowRandomPoints(InputPtr input, ModelPtr model, Parameter* para) const
{
int num = random_points_.size();
for (int t = 0; t < num; ++t)
{
int id = random_points_[t];
ivec2 pos = para->ComputePosition(id);
//vec3 rgb = input_image->GetColor(Point(pos[0], pos[1]));
//Vec3b color(static_cast<uchar>(rgb[2]), static_cast<uchar>(rgb[1]), static_cast<uchar>(rgb[0]));
Vec3b color(0, 0, 0);
model->WriteColor(pos[0], pos[1], color);
}
}
double Optimizer::ComputeCost(InputPtr input_image, ModelPtr model, Alpha& alpha) const
{
double function_value = 0;
int random_num = 1000;
vector<int> triangle_ids;
model->GenerateRandomNumbers(random_num, triangle_ids);
//int random_num = random_points_.size();
for (int i = 0; i < random_num; ++i)
{
int id = triangle_ids[i];
//int id = random_points_[i];
ivec2 pos = alpha.ComputePosition(id);
vec3 model_color = alpha.ComputeColor(id); // in RGB order
vec3 input_color = input_image->GetColor(Point(pos[0], pos[1])); // in RGB order
vec3 diff = (model_color - input_color);
function_value += dot(diff, diff);
}
return 40.0/1000*function_value;
}
void Optimizer::FitAll(mat& alpha,
mat& beta,
mat& rho,
mat& lamda,
InputPtr input,
ModelPtr model,
MeshPtr mesh,
ShapePtr shape,
TexturePtr texture)
{
mat alpha_gradient = mat(PrincipalNum, 1, fill::zeros);
mat alpha_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
mat beta_gradient = mat(PrincipalNum, 1, fill::zeros);
mat beta_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
mat rho_gradient = mat(RhoNum, 1, fill::zeros);
mat rho_hessian_inv = mat(RhoNum, RhoNum, fill::zeros);
mat lamda_gradient = mat(LamdaNum, 1, fill::zeros);
mat lamda_hessian_inv = mat(LamdaNum, LamdaNum, fill::zeros);
vec step(RhoNum, 1);
step.rows(0, 2).fill(0.00000006);
step.rows(3, 5).fill(0.0002);
step.rows(6, 6).fill(2.0);
mat step_mat = diagmat(step);
SetStartingValue(rho, lamda);
// 2500 1000 700 500 300 200
array<double, 6> weight_intensity = { { 1.0 / 1000, 1.0 / 900, 1.0 / 800, 1.0 / 700, 1.0 / 500, 1.0 / 300 } };
array<int, 6> para_num = { { 10, 15, 25, 35, 55, 99 } }; // 20 40
array<int, 6> iterations = { { 1000, 1000, 1000, 1000, 1000, 1000 } };
//array<double, 4> weight_intensity = { { 1.0 / 900, 1.0 / 700, 1.0 / 500, 1.0 / 400 } };
//array<int, 4> para_num = { { 10, 20, 40, 99 } }; // 20 40
//array<int, 4> iterations = { { 1000, 1000, 800, 600 } };
int counter = 0;
for (int c = 0; c < 6; ++c)
{
for (int l = 0; l < iterations[c]; ++l)
{
if (counter == 0 || counter % 1000 == 0)
{
Face3dModel face3d_model(shape, texture);
mesh = face3d_model.Construction(alpha, beta);
TwoPassZbuffer(rho, lamda, mesh, model);
model->EnableIterator();
model->InitialRandomGenerator();
//GenerateRandomPoints(model, HessianRandomNum);
//for (int i = 0; i < para_num[c]; ++i)
//{
// double variance = shape->GetVariance(i);
// mat alpha1 = alpha;
// alpha1[i] -= H;
// Alpha alpha_para1(alpha1, input, model, mesh, shape, texture);
// double first_derivative1 = weight_feature[c] * ComputeLandmarkGradient(input, &alpha_para1, i) +
// weight_intensity[c] * ComputeIntensityGradient(input, &alpha_para1, i);
// mat alpha2 = alpha;
// alpha2[i] += H;
// Alpha alpha_para2(alpha2, input, model, mesh, shape, texture);
// double first_derivative2 = weight_feature[c] * ComputeLandmarkGradient(input, &alpha_para2, i) +
// weight_intensity[c] * ComputeIntensityGradient(input, &alpha_para2, i);
// double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
// alpha_hessian_inv(i, i) = 1 / (second_derivative +2 / variance);
//}
/* for (int i = 0; i < para_num[c]; ++i)
{
double variance = texture->GetVariance(i);
mat beta1 = beta;
beta1[i] -= H;
Beta beta_para1(beta1, model, mesh, shape, texture);
double first_derivative1 = weight_intensity[c] * ComputeIntensityGradient(input, &beta_para1, i);
mat beta2 = beta;
beta2[i] += H;
Beta beta_para2(beta2, model, mesh, shape, texture);
double first_derivative2 = weight_intensity[c] * ComputeIntensityGradient(input, &beta_para2, i);
double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
beta_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);
}*/
//for (int i = 0; i < RhoNum; ++i)
//{
// double variance = GetRhoVariance(i);
// mat rho1 = rho;
// rho1[i] -= H;
// Rho rho_para1(rho1, input, model, mesh, shape, texture);
// double first_derivative1 =/* weight_feature[c] * ComputeLandmarkGradient(input, &rho_para1, i) +*/
// weight_intensity[c] * ComputeIntensityGradient(input, &rho_para1, i);
// mat rho2 = rho;
// rho2[i] += H;
// Rho rho_para2(rho2, input, model, mesh, shape, texture);
// double first_derivative2 = /*weight_feature[c] * ComputeLandmarkGradient(input, &rho_para2, i) +*/
// weight_intensity[c] * ComputeIntensityGradient(input, &rho_para2, i);
// double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
// rho_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);
//}
//for (int i = 0; i < LamdaNum - 7; ++i)
//{
// double variance = GetLamdaVariance(i);
// mat lamda1 = lamda;
// lamda1[i] -= H;
// Lamda lamda_para1(lamda1, model, mesh, shape, texture);
// double first_derivative1 = weight_intensity[c] * ComputeIntensityGradient(input, &lamda_para1, i);
// mat lamda2 = lamda;
// lamda2[i] += H;
// Lamda lamda_para2(lamda2, model, mesh, shape, texture);
// double first_derivative2 = weight_intensity[c] * ComputeIntensityGradient(input, &lamda_para2, i);
// double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
// lamda_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);
//}
}
Alpha alpha_para(alpha, input, model, mesh, shape, texture);
Beta beta_para(beta, model, mesh, shape, texture);
Rho rho_para(rho, input, model, mesh, shape, texture);
Lamda lamda_para(lamda, model, mesh, shape, texture);
GenerateRandomPoints(model, GradientRandomNum);
//double function_value = 0;
//function_value = ComputeCost(input, model, alpha_para);
//ofstream cost;
//cost.open("all_cost", ios::app);
//cost << function_value << "\n";
//cost.close();
for (int i = 0; i < para_num[c]; ++i)
{
double variance = shape->GetVariance(i);
alpha_gradient[i] = /*weight_feature[c] * ComputeLandmarkGradient(input, &alpha_para, i) +*/
weight_intensity[c] * ComputeIntensityGradient(input, &alpha_para, i) +2 * alpha[i] / variance;
}
for (int i = 0; i < para_num[c]; ++i)
{
double variance = texture->GetVariance(i);
beta_gradient[i] = weight_intensity[c] * ComputeIntensityGradient(input, &beta_para, i) + 2 * beta[i] / variance;
}
for (int i = 0; i < RhoNum; ++i)
{
//double variance = GetRhoVariance(i);
//double mean = GetRhoMean(i);
rho_gradient[i] = /*weight_feature[c] * ComputeLandmarkGradient(input, &rho_para, i) +*/
weight_intensity[c] * ComputeIntensityGradient(input, &rho_para, i);// +2 * (rho[i] - mean) / variance;
}
for (int i = 0; i < LamdaNum; ++i)
{
double variance = GetLamdaVariance(i);
double mean = GetLamdaMean(i);
lamda_gradient[i] = weight_intensity[c] * ComputeIntensityGradient(input, &lamda_para, i) +2 * (lamda[i] - mean) / variance;
}
//alpha -= 0.00003*alpha_hessian_inv*alpha_gradient; // 0.01 0.00003
//beta -= 0.01*beta_hessian_inv*beta_gradient;
//lamda -= 0.03*lamda_hessian_inv*lamda_gradient;
//rho -= 0.0001*rho_hessian_inv*rho_gradient;
alpha -= alpha_para.Step*alpha_gradient;
beta -= beta_para.Step*beta_gradient;
rho -= step_mat*rho_gradient; // gradient descent
lamda -= lamda_para.Step*lamda_gradient;
++counter;
}
Face3dModel face3d_model(shape, texture);
mesh = face3d_model.Construction(alpha, beta);
VisualizeResult(rho, lamda, input, mesh, "segmented", c);
}
}
TEST(solver_interface, setup_problem) {
ModelPtr solver = createModel(SOLVER_GUROBI);
vector<Var> vars;
for (int i=0; i < 3; ++i) {
char namebuf[5];
sprintf(namebuf, "v%i", i);
vars.push_back(solver->addVar(namebuf));
}
solver->update();
AffExpr aff;
cout << aff << endl;
for (int i=0; i < 3; ++i) {
exprInc(aff, vars[i]);
solver->setVarBounds(vars[i], 0, 10);
}
aff.constant -= 3;
cout << aff << endl;
QuadExpr affsquared = exprSquare(aff);
solver->setObjective(affsquared);
solver->update();
LOG_INFO("objective: %s", CSTR(affsquared));
LOG_INFO("please manually check that /tmp/solver-interface-test.lp matches this");
solver->writeToFile("/tmp/solver-interface-test.lp");
solver->optimize();
vector<double> soln(3);
for (int i=0; i < 3; ++i) {
soln[i] = solver->getVarValue(vars[i]);
}
EXPECT_NEAR(aff.value(soln), 0, 1e-6);
solver->removeVar(vars[2]);
solver->update();
EXPECT_EQ(solver->getVars().size(), 2);
}
void
Scene::parse_istream(std::istream& in)
{
// This is not a fully featured .obj file reader, it just takes some
// inspiration from it:
// http://www.martinreddy.net/gfx/3d/OBJ.spec
std::unordered_map<std::string, SceneNode*> nodes;
std::unordered_map<std::string, std::unique_ptr<SceneNode> > unattached_children;
std::string name;
std::string parent;
std::string material = "phong";
glm::vec3 location(0.0f, 0.0f, 0.0f);
glm::quat rotation(1.0f, 0.0f, 0.0f, 0.0f);
glm::vec3 scale(1.0f, 1.0f, 1.0f);
std::vector<glm::vec3> normal;
std::vector<glm::vec3> position;
std::vector<glm::vec3> texcoord;
std::vector<int> index;
std::vector<glm::vec4> bone_weight;
std::vector<glm::ivec4> bone_index;
std::vector<int> bone_count;
auto commit_object = [&]{
if (!name.empty())
{
ModelPtr model;
if (!position.empty())
{
// fill in some texcoords if there aren't enough
if (texcoord.size() < position.size())
{
texcoord.resize(position.size());
for(FaceLst::size_type i = position.size()-1; i < texcoord.size(); ++i)
{
texcoord[i] = glm::vec3(0.0f, 0.0f, 0.0f);
}
}
{
// create Mesh
std::unique_ptr<Mesh> mesh(new Mesh(GL_TRIANGLES));
mesh->attach_float_array("position", position);
mesh->attach_float_array("texcoord", texcoord);
mesh->attach_float_array("normal", normal);
mesh->attach_element_array(index);
if (!bone_weight.empty() && !bone_index.empty())
{
mesh->attach_float_array("bone_weight", bone_weight);
mesh->attach_int_array("bone_index", bone_index);
}
// create Model
model = std::make_shared<Model>();
model->add_mesh(std::move(mesh));
if (boost::algorithm::ends_with(material, ".material"))
{
model->set_material(MaterialFactory::get().from_file(m_directory / boost::filesystem::path(material)));
}
else
{
model->set_material(MaterialFactory::get().create(material));
}
}
}
// create SceneNode
{
std::unique_ptr<SceneNode> node(new SceneNode(name));
node->set_position(location);
node->set_orientation(rotation);
node->set_scale(scale);
if (model)
{
node->attach_model(model);
}
if (nodes.find(name) != nodes.end())
{
throw std::runtime_error("duplicate object name: " + name);
}
nodes[name] = node.get();
if (parent.empty())
{
m_node->attach_child(std::move(node));
}
else
{
unattached_children[parent] = std::move(node);
}
}
// clear for the next mesh
name.clear();
parent.clear();
normal.clear();
texcoord.clear();
position.clear();
index.clear();
location = glm::vec3(0.0f, 0.0f, 0.0f);
rotation = glm::quat(1.0f, 0.0f, 0.0f, 0.0f);
scale = glm::vec3(1.0f, 1.0f, 1.0f);
}
};
std::string line;
int line_number = 0;
while(std::getline(in, line))
{
line_number += 1;
boost::tokenizer<boost::char_separator<char> > tokens(line, boost::char_separator<char>(" ", ""));
auto it = tokens.begin();
if (it != tokens.end())
{
#define INCR_AND_CHECK { \
++it; \
if (it == tokens.end()) \
{ \
throw std::runtime_error((boost::format("not enough tokens at line %d") % line_number).str()); \
} \
}
try
{
if (*it == "o")
{
// object
commit_object();
INCR_AND_CHECK;
log_debug("object: '%s'", *it);
name = *it;
}
else if (*it == "g")
{
// group
}
else if (*it == "parent")
{
INCR_AND_CHECK;
parent = *it;
}
else if (*it == "mat")
{
INCR_AND_CHECK;
material = *it;
}
else if (*it == "loc")
{
INCR_AND_CHECK;
location.x = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
location.y = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
location.z = boost::lexical_cast<float>(*it);
}
else if (*it == "rot")
{
INCR_AND_CHECK;
rotation.w = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
rotation.x = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
rotation.y = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
rotation.z = boost::lexical_cast<float>(*it);
}
else if (*it == "scale")
{
INCR_AND_CHECK;
scale.x = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
scale.y = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
scale.z = boost::lexical_cast<float>(*it);
}
else if (*it == "v")
{
glm::vec3 v;
INCR_AND_CHECK;
v.x = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
v.y = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
v.z = boost::lexical_cast<float>(*it);
position.push_back(v);
}
else if (*it == "vt")
{
glm::vec3 vt;
INCR_AND_CHECK;
vt.s = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
vt.t = boost::lexical_cast<float>(*it);
texcoord.push_back(vt);
}
else if (*it == "vn")
{
glm::vec3 vn;
INCR_AND_CHECK;
vn.x = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
vn.y = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
vn.z = boost::lexical_cast<float>(*it);
normal.push_back(vn);
}
else if (*it == "bw")
{
glm::vec4 bw;
INCR_AND_CHECK;
bw.x = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
bw.y = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
bw.z = boost::lexical_cast<float>(*it);
INCR_AND_CHECK;
bw.w = boost::lexical_cast<float>(*it);
bone_weight.push_back(bw);
}
else if (*it == "bi")
{
glm::ivec4 bi;
INCR_AND_CHECK;
bi.x = boost::lexical_cast<int>(*it);
INCR_AND_CHECK;
bi.y = boost::lexical_cast<int>(*it);
INCR_AND_CHECK;
bi.z = boost::lexical_cast<int>(*it);
INCR_AND_CHECK;
bi.w = boost::lexical_cast<int>(*it);
bone_index.push_back(bi);
}
else if (*it == "f")
{
INCR_AND_CHECK;
index.push_back(boost::lexical_cast<int>(*it));
INCR_AND_CHECK;
index.push_back(boost::lexical_cast<int>(*it));
INCR_AND_CHECK;
index.push_back(boost::lexical_cast<int>(*it));
}
else if ((*it)[0] == '#')
{
// ignore comments
}
else
{
throw std::runtime_error((boost::format("unhandled token %s") % *it).str());
}
}
catch(const std::exception& err)
{
throw std::runtime_error((boost::format("unknown:%d: %s") % line_number % err.what()).str());
}
}
}
commit_object();
// reconstruct parent/child relationships
for(auto& it : unattached_children)
{
auto p = nodes.find(it.first);
if (p == nodes.end())
{
throw std::runtime_error("parent not found: " + it.first);
}
else
{
p->second->attach_child(std::move(it.second));
}
}
}
void
Compositor::render(Viewer& viewer)
{
// render the world, twice if stereo is enabled
if (true)
{
OpenGLState state;
#ifndef HAVE_OPENGLES2
if (m_render_shadowmap)
{
g_shadowmap->bind();
render_shadowmap(viewer);
g_shadowmap->unbind();
}
#endif
if (m_stereo_mode == StereoMode::None)
{
m_renderbuffer1->bind();
render_scene(viewer, Stereo::Center);
m_renderbuffer1->unbind();
m_renderbuffer1->blit(*m_framebuffer1);
}
else
{
m_renderbuffer1->bind();
render_scene(viewer, Stereo::Left);
m_renderbuffer1->unbind();
m_renderbuffer2->bind();
render_scene(viewer, Stereo::Right);
m_renderbuffer2->unbind();
m_renderbuffer1->blit(*m_framebuffer1);
m_renderbuffer2->blit(*m_framebuffer2);
}
}
// composit the final image
if (true)
{
OpenGLState state;
MaterialPtr material = std::make_shared<Material>();
{ // setup material
material->set_program(m_composition_prog);
material->set_uniform("MVP", UniformSymbol::ModelViewProjectionMatrix);
material->set_uniform("barrel_power", m_barrel_power);
material->set_uniform("left_eye", 0);
material->set_uniform("right_eye", 1);
if (!viewer.m_cfg.m_show_calibration)
{
material->set_texture(0, m_framebuffer1->get_color_texture());
material->set_texture(1, m_framebuffer2->get_color_texture());
}
else
{
material->set_texture(0, m_calibration_left_texture);
material->set_texture(1, m_calibration_right_texture);
}
m_viewport_offset = {0, 0};
switch(m_stereo_mode)
{
case StereoMode::Cybermaxx:
m_viewport_offset = {-41, 16};
m_composition_prog = m_cybermaxx_prog;
break;
case StereoMode::CrossEye:
m_composition_prog = m_crosseye_prog;
break;
case StereoMode::Anaglyph:
m_composition_prog = m_anaglyph_prog;
break;
case StereoMode::Depth:
material->set_texture(0, m_framebuffer1->get_depth_texture());
m_composition_prog = m_depth_prog;
break;
case StereoMode::Newsprint:
m_composition_prog = m_newsprint_prog;
break;
default:
m_composition_prog = m_mono_prog;
break;
}
} // setup material
ModelPtr model = std::make_shared<Model>();
model->add_mesh(Mesh::create_rect(0.0f, 0.0f, m_screen_w, m_screen_h, -20.0f));
model->set_material(material);
Camera camera;
camera.ortho(0, m_screen_w, m_screen_h, 0.0f, 0.1f, 10000.0f);
SceneManager mgr;
mgr.get_world()->attach_model(model);
RenderContext ctx(camera, mgr.get_world());
glViewport(m_viewport_offset.x, m_viewport_offset.y, m_screen_w, m_screen_h);
glClearColor(0.0, 0.0, 0.0, 1.0);
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
mgr.render(camera);
render_menu(ctx, viewer);
}
assert_gl("display:exit()");
}
void Optimizer::FitSegments(mat& alpha,
mat& beta,
mat& rho,
mat& lamda,
InputPtr input,
ModelPtr model,
MeshPtr mesh,
ShapePtr shape,
TexturePtr texture)
{
mat eye_alpha = alpha;
mat eye_beta = beta;
mat nose_alpha = alpha;
mat nose_beta = beta;
mat mouth_alpha = alpha;
mat mouth_beta = beta;
mat rest_alpha = alpha;
mat rest_beta = beta;
Face3dModel face3d_model(shape, texture);
mesh = face3d_model.Construction(alpha, beta);
TwoPassZbuffer(rho, lamda, mesh, model, true); // cal segment two pass z-buffer
for (int c = 0; c < 3; ++c)
{
model->EnableIterator(ModelImage::NOSE);
FitNose(nose_alpha, nose_beta, rho, lamda, input, model, mesh, shape, texture);
model->EnableIterator(ModelImage::EYE);
FitEye(eye_alpha, eye_beta, rho, lamda, input, model, mesh, shape, texture);
model->EnableIterator(ModelImage::MOUTH);
FitMouth(mouth_alpha, mouth_beta, rho, lamda, input, model, mesh, shape, texture);
model->EnableIterator(ModelImage::REST);
FitRest(rest_alpha, rest_beta, rho, lamda, input, model, mesh, shape, texture);
}
mat all_alpha(PrincipalNum,SegmentsNum);
all_alpha.cols(0, 0) = nose_alpha;
all_alpha.cols(1, 1) = eye_alpha;
all_alpha.cols(2, 2) = mouth_alpha;
all_alpha.cols(3, 3) = rest_alpha;
mat all_beta(PrincipalNum, SegmentsNum);
all_beta.cols(0, 0) = nose_beta;
all_beta.cols(1, 1) = eye_beta;
all_beta.cols(2, 2) = mouth_beta;
all_beta.cols(3, 3) = rest_beta;
mesh = face3d_model.Construction(all_alpha, all_beta);
mesh->Blend();
rho.quiet_save("rho_para", arma_binary);
lamda.quiet_save("lamda_para", arma_binary);
}
void Optimizer::FitFirstPrincipalComponents(mat& alpha,
mat& beta,
mat& rho,
mat& lamda,
InputPtr input,
ModelPtr model,
MeshPtr mesh,
ShapePtr shape,
TexturePtr texture,
const cv::Mat& ready)
{
mat alpha_gradient = mat(PrincipalNum, 1, fill::zeros);
mat alpha_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
mat beta_gradient = mat(PrincipalNum, 1, fill::zeros);
mat rho_gradient = mat(RhoNum, 1, fill::zeros);
mat lamda_gradient = mat(LamdaNum, 1, fill::zeros);
vec step(RhoNum, 1);
step.rows(0, 2).fill(0.00000006);
step.rows(3, 5).fill(0.0002);
step.rows(6, 6).fill(2.0);
mat step_mat = diagmat(step);
// down sampled version
cv::Mat down_sampled;
pyrDown(ready, down_sampled, Size(IMAGE_WIDTH / 2, IMAGE_HEIGHT / 2));
input = make_shared<InputImage>(down_sampled);
model->SetSize(IMAGE_WIDTH / 2, IMAGE_HEIGHT / 2);
rho[FOCAL] /= 2;
//SetStartingValue(rho, lamda);
double para_num = 10;
double weight = 1.0 / 900;
int counter = 0;
while (counter < 1000)
{
Alpha alpha_para(alpha, input, model, mesh, shape, texture);
Beta beta_para(beta, model, mesh, shape, texture);
Rho rho_para(rho, input, model, mesh, shape, texture);
Lamda lamda_para(lamda, model, mesh, shape, texture);
if (counter == 0 || counter % 1000 == 0)
{
Face3dModel face3d_model(shape, texture);
mesh = face3d_model.Construction(alpha, beta);
TwoPassZbuffer(rho, lamda, mesh, model);
model->EnableIterator();
model->InitialRandomGenerator();
//GenerateRandomPoints(model, HessianRandomNum);
//for (int i = 0; i < para_num; ++i)
//{
// double variance = shape->GetVariance(i);
// mat alpha1 = alpha;
// alpha1[i] -= H;
// Alpha alpha_para1(alpha1, input, model, mesh, shape, texture);
// double first_derivative1 = weight * ComputeIntensityGradient(input, &alpha_para1, i) + 2 * alpha1[i] / variance;
// mat alpha2 = alpha;
// alpha2[i] += H;
// Alpha alpha_para2(alpha2, input, model, mesh, shape, texture);
// double first_derivative2 = weight * ComputeIntensityGradient(input, &alpha_para2, i) + 2 * alpha2[i] / variance;
// double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
// alpha_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);
//}
}
// Generate random points
GenerateRandomPoints(model, GradientRandomNum);
double function_value = 0;
function_value = ComputeCost(input, model, alpha_para);
ofstream cost;
cost.open("first_cost", ios::app);
cost << function_value << "\n";
cost.close();
for (int i = 0; i < para_num; ++i)
{
double variance = shape->GetVariance(i);
alpha_gradient[i] = weight * ComputeIntensityGradient(input, &alpha_para, i) + 2 * alpha[i] / variance;
}
//for (int i = 0; i < 10; ++i)
//{
// double variance = texture->GetVariance(i);
// beta_gradient[i] = weight *ComputeIntensityGradient(input, &beta_para, i) + 2 * beta[i] / variance;
//}
//for (int i = 0; i < RhoNum; ++i)
//{
// double first_derivative1 = 1.0 / 1000*ComputeIntensityGradient(input, &rho_para, i);
// //double variance = rho_para.GetVariance(i);
// //double mean = rho_para.GetMean(i);
// rho_gradient[i] = first_derivative1;// +2 * (rho[i] - mean) / variance;
//}
//for (int i = 0; i < LamdaNum-7; ++i)
//{
// double first_derivative1 = 1.0 / 1000 * ComputeIntensityGradient(input, &lamda_para, i);
// //double variance = lamda_para.GetVariance(i);
// //double mean = lamda_para.GetMean(i);
// lamda_gradient[i] = first_derivative1; //+2 * (lamda[i] - mean) / variance;
//}
alpha -= alpha_para.Step*alpha_gradient;
//beta -= beta_para.Step*beta_gradient;
//rho -= step_mat*rho_gradient;
//lamda -= lamda_para.Step*lamda_gradient;
//alpha -= 0.00008*alpha_hessian_inv*alpha_gradient;
++counter;
}
// restore original version
input= make_shared<InputImage>(ready);
model->SetSize(IMAGE_WIDTH, IMAGE_HEIGHT);
rho[FOCAL] *= 2;
Face3dModel face3d_model(shape, texture);
mesh = face3d_model.Construction(alpha, beta);
VisualizeResult(rho, lamda, input, mesh, "first");
}
void Optimizer::FitIllumination(mat& alpha,
mat& beta,
mat& rho,
mat& lamda,
InputPtr input,
ModelPtr model,
MeshPtr mesh,
ShapePtr shape,
TexturePtr texture,
const cv::Mat& ready)
{
mat lamda_gradient=mat(LamdaNum, 1,fill::zeros);
mat lamda_hessian_inv = mat(LamdaNum, LamdaNum, fill::zeros);
Face3dModel face3d_model(shape, texture);
mesh = face3d_model.Construction(alpha, beta);
//VisualizeResult(rho, lamda, input, mesh, 0);
TwoPassZbuffer(rho, lamda, mesh, model);
model->EnableIterator();
model->InitialRandomGenerator();
// down sampled version
cv::Mat down_sampled;
pyrDown(ready, down_sampled, Size(IMAGE_WIDTH / 2, IMAGE_HEIGHT / 2));
input = make_shared<InputImage>(down_sampled);
model->SetSize(IMAGE_WIDTH / 2, IMAGE_HEIGHT / 2);
rho[FOCAL] /= 2;
double weight = 1.0 / 1000;
int counter = 0;
while (counter < 500)
{
Alpha alpha_para(alpha, input, model, mesh, shape, texture);
Beta beta_para(beta, model, mesh, shape, texture);
Rho rho_para(rho, input, model, mesh, shape, texture);
Lamda lamda_para(lamda, model, mesh, shape, texture);
// Generate random points
//if (counter == 0 || counter % 1000 == 0)
//{
// GenerateRandomPoints(model, HessianRandomNum);
// for (int i = 0; i < LamdaNum - 7; ++i)
// {
// mat lamda1 = lamda;
// lamda1[i] -= H;
// Lamda lamda_para1(lamda1, model, mesh, shape, texture);
// double first_derivative1 = weight*ComputeIntensityGradient(input, &lamda_para1, i);
// mat lamda2 = lamda;
// lamda2[i] += H;
// Lamda lamda_para2(lamda2, model, mesh, shape, texture);
// double first_derivative2 = weight* ComputeIntensityGradient(input, &lamda_para2, i);
// double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
// lamda_hessian_inv(i, i) = 1 / second_derivative;
// }
//}
GenerateRandomPoints(model, GradientRandomNum);
//double function_value = 0;
//function_value = ComputeCost(input, model, alpha_para);
//ofstream cost;
//cost.open("illumination_cost", ios::app);
//cost << function_value << "\n";
//cost.close();
for (int i = 0; i < LamdaNum; ++i)
{
lamda_gradient[i] = weight*ComputeIntensityGradient(input, &lamda_para, i);
}
lamda -= lamda_para.Step*lamda_gradient;
//lamda -= 0.5*lamda_hessian_inv*lamda_gradient;
++counter;
}
// restore original version
input= make_shared<InputImage>(ready);
model->SetSize(IMAGE_WIDTH, IMAGE_HEIGHT);
rho[FOCAL] *= 2;
VisualizeResult(rho, lamda, input, mesh, "illumination");
}