<commit_before>//=======================================================================
// Copyright Baptiste Wicht 2011.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <memory>

#include <boost/variant.hpp>

#include "tac/Printer.hpp"
#include "tac/Program.hpp"

#include "VisitorUtils.hpp"
#include "Utils.hpp"

using namespace eddic;

namespace {

struct ArgumentToString : public boost::static_visitor<std::string> {
   std::string operator()(std::shared_ptr<Variable>& variable) const {
        return variable->name();
   }

   std::string operator()(int& integer) const {
        return toString(integer);
   }

   std::string operator()(double& float_) const {
        return toString(float_);
   }

   std::string operator()(std::string& str) const {
        return str;
   }
};

std::string printArgument(tac::Argument& arg){
    return visit(ArgumentToString(), arg);
}

struct DebugVisitor : public boost::static_visitor<> {
    void operator()(tac::Program& program){
        std::cout << "TAC Program " << std::endl << std::endl;

        visit_each_non_variant(*this, program.functions);
    }

    void operator()(std::shared_ptr<tac::Function> function){
        std::cout << "Function " << function->getName() << std::endl;

        visit_each(*this, function->getStatements());
        visit_each_non_variant(*this, function->getBasicBlocks());

        std::cout << std::endl;
    }

    void operator()(std::shared_ptr<tac::BasicBlock>& block){
        std::cout << "B" << block->index << "->" << std::endl;

        visit_each(*this, block->statements);
    }

    void operator()(tac::Statement& statement){
        visit(*this, statement);
    }

    void operator()(std::shared_ptr<tac::Quadruple>& quadruple){
        if(quadruple->op == tac::Operator::ASSIGN){
            std::cout << "\t" << quadruple->result->name() << " = (normal) " << printArgument(*quadruple->arg1) << std::endl;
        } else if(quadruple->op == tac::Operator::FASSIGN){
            std::cout << "\t" << quadruple->result->name() << " = (float) " << printArgument(*quadruple->arg1) << std::endl;
        } else {
            tac::Operator op = quadruple->op;

            if(op == tac::Operator::ADD || op == tac::Operator::FADD){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " + " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::SUB || op == tac::Operator::FSUB){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " - " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::MUL || op == tac::Operator::FMUL){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " * " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::DIV || op == tac::Operator::FDIV){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " / " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::MOD){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " % " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::EQUALS || op == tac::Operator::FE){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " == " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::NOT_EQUALS || op == tac::Operator::FNE){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " != " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::GREATER || op == tac::Operator::FG){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " > " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::GREATER_EQUALS || op == tac::Operator::FGE){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " >= " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::LESS || op == tac::Operator::FL){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " < " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::LESS_EQUALS || op == tac::Operator::FLE){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " <= " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::MINUS){
                std::cout << "\t" << quadruple->result->name() << " = - " << printArgument(*quadruple->arg1) << std::endl;
            } else if(op == tac::Operator::DOT){
                std::cout << "\t" << quadruple->result->name() << " = (" << printArgument(*quadruple->arg1) << ")" << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::DOT_ASSIGN){
                std::cout << "\t(" << quadruple->result->name() << ")" << printArgument(*quadruple->arg1) << " = " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::ARRAY){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " [" << printArgument(*quadruple->arg2) << "]" << std::endl;
            } else if(op == tac::Operator::ARRAY_ASSIGN){
                std::cout << "\t" << quadruple->result->name() << "[" << printArgument(*quadruple->arg1) << "] = " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::PARAM){
                std::cout << "\tparam " << printArgument(*quadruple->arg1) << std::endl;
            } else if(op == tac::Operator::RETURN){
                std::cout << "\treturn";

                if(quadruple->arg1){
                    std::cout << " " << printArgument(*quadruple->arg1);
                }

                if(quadruple->arg2){
                    std::cout << ", " << printArgument(*quadruple->arg2);
                }

                std::cout << std::endl;
            }
        }
    }

    template<typename T>
    std::string printTarget(std::shared_ptr<T>& ifFalse){
        if(ifFalse->block){
            return "B" + toString(ifFalse->block->index);
        } else {
            return ifFalse->label;
        }
    }

    void operator()(std::shared_ptr<tac::IfFalse>& ifFalse){
        if(ifFalse->op){
            auto op = *ifFalse->op;
            if(op == tac::BinaryOperator::EQUALS || op == tac::BinaryOperator::FE){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " == " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(op == tac::BinaryOperator::NOT_EQUALS || op == tac::BinaryOperator::FNE){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " != " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(op == tac::BinaryOperator::LESS || op == tac::BinaryOperator::FL){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " < " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(op == tac::BinaryOperator::LESS_EQUALS || op == tac::BinaryOperator::FLE){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " <= " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(op == tac::BinaryOperator::GREATER || op == tac::BinaryOperator::FG){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " > " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(op == tac::BinaryOperator::GREATER_EQUALS || op == tac::BinaryOperator::FGE){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " >= " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            }
        } else {
            std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " goto " << printTarget(ifFalse) << std::endl;
        }
    }

    void operator()(std::shared_ptr<tac::If>& ifFalse){
        if(ifFalse->op){
            if(*ifFalse->op == tac::BinaryOperator::EQUALS){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " == " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(*ifFalse->op == tac::BinaryOperator::NOT_EQUALS){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " != " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(*ifFalse->op == tac::BinaryOperator::LESS){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " < " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(*ifFalse->op == tac::BinaryOperator::LESS_EQUALS){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " <= " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(*ifFalse->op == tac::BinaryOperator::GREATER){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " > " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(*ifFalse->op == tac::BinaryOperator::GREATER_EQUALS){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " >= " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            }
        } else {
            std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " goto " << printTarget(ifFalse) << std::endl;
        }
    }

    void operator()(std::shared_ptr<tac::Goto>& goto_){
        std::cout << "\tgoto " << printTarget(goto_) << std::endl;
    }

    void operator()(tac::NoOp&){
        std::cout << "\tno-op" << std::endl;
    }

    void operator()(std::shared_ptr<tac::Call>& call){
        std::cout << "\t";

        if(call->return_){
            std::cout << call->return_->name();
        }

        if(call->return2_){
            std::cout << ", " << call->return2_->name();
        }

        if(call->return_ || call->return2_){
            std::cout << " = ";
        }

        std::cout << "call " << call->function << std::endl;
    }

    void operator()(std::string& label){
        std::cout << "\t" << label << ":" << std::endl;
    }
};

} //end of anonymous namespace

void tac::Printer::print(tac::Program& program) const {
   DebugVisitor visitor;
   visitor(program);
}

void tac::Printer::print(tac::Statement& statement) const {
   DebugVisitor visitor;
   visit(visitor, statement);
}
<commit_msg>Enhance the printer<commit_after>//=======================================================================
// Copyright Baptiste Wicht 2011.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <memory>

#include <boost/variant.hpp>

#include "tac/Printer.hpp"
#include "tac/Program.hpp"

#include "VisitorUtils.hpp"
#include "Utils.hpp"

using namespace eddic;

namespace {

struct ArgumentToString : public boost::static_visitor<std::string> {
   std::string operator()(std::shared_ptr<Variable>& variable) const {
        return variable->name();
   }

   std::string operator()(int& integer) const {
        return toString(integer);
   }

   std::string operator()(double& float_) const {
        return toString(float_);
   }

   std::string operator()(std::string& str) const {
        return str;
   }
};

std::string printArgument(tac::Argument& arg){
    return visit(ArgumentToString(), arg);
}

struct DebugVisitor : public boost::static_visitor<> {
    void operator()(tac::Program& program){
        std::cout << "TAC Program " << std::endl << std::endl;

        visit_each_non_variant(*this, program.functions);
    }

    void operator()(std::shared_ptr<tac::Function> function){
        std::cout << "Function " << function->getName() << std::endl;

        visit_each(*this, function->getStatements());
        visit_each_non_variant(*this, function->getBasicBlocks());

        std::cout << std::endl;
    }

    void operator()(std::shared_ptr<tac::BasicBlock>& block){
        std::cout << "B" << block->index << "->" << std::endl;

        visit_each(*this, block->statements);
    }

    void operator()(tac::Statement& statement){
        visit(*this, statement);
    }

    void operator()(std::shared_ptr<tac::Quadruple>& quadruple){
        if(quadruple->op == tac::Operator::ASSIGN){
            std::cout << "\t" << quadruple->result->name() << " = (normal) " << printArgument(*quadruple->arg1) << std::endl;
        } else if(quadruple->op == tac::Operator::FASSIGN){
            std::cout << "\t" << quadruple->result->name() << " = (float) " << printArgument(*quadruple->arg1) << std::endl;
        } else {
            tac::Operator op = quadruple->op;

            if(op == tac::Operator::ADD){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " + " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::FADD){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " + (float) " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::SUB){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " - " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::FSUB){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " - (float) " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::MUL){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " * " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::FMUL){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " * (float) " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::DIV){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " / " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::FDIV){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " / (float) " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::MOD){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " % " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::EQUALS || op == tac::Operator::FE){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " == " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::NOT_EQUALS || op == tac::Operator::FNE){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " != " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::GREATER || op == tac::Operator::FG){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " > " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::GREATER_EQUALS || op == tac::Operator::FGE){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " >= " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::LESS || op == tac::Operator::FL){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " < " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::LESS_EQUALS || op == tac::Operator::FLE){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " <= " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::MINUS){
                std::cout << "\t" << quadruple->result->name() << " = - " << printArgument(*quadruple->arg1) << std::endl;
            } else if(op == tac::Operator::DOT){
                std::cout << "\t" << quadruple->result->name() << " = (" << printArgument(*quadruple->arg1) << ")" << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::DOT_ASSIGN){
                std::cout << "\t(" << quadruple->result->name() << ")" << printArgument(*quadruple->arg1) << " = " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::ARRAY){
                std::cout << "\t" << quadruple->result->name() << " = " << printArgument(*quadruple->arg1) << " [" << printArgument(*quadruple->arg2) << "]" << std::endl;
            } else if(op == tac::Operator::ARRAY_ASSIGN){
                std::cout << "\t" << quadruple->result->name() << "[" << printArgument(*quadruple->arg1) << "] = " << printArgument(*quadruple->arg2) << std::endl;
            } else if(op == tac::Operator::PARAM){
                std::cout << "\tparam " << printArgument(*quadruple->arg1) << std::endl;
            } else if(op == tac::Operator::RETURN){
                std::cout << "\treturn";

                if(quadruple->arg1){
                    std::cout << " " << printArgument(*quadruple->arg1);
                }

                if(quadruple->arg2){
                    std::cout << ", " << printArgument(*quadruple->arg2);
                }

                std::cout << std::endl;
            }
        }
    }

    template<typename T>
    std::string printTarget(std::shared_ptr<T>& ifFalse){
        if(ifFalse->block){
            return "B" + toString(ifFalse->block->index);
        } else {
            return ifFalse->label;
        }
    }

    void operator()(std::shared_ptr<tac::IfFalse>& ifFalse){
        if(ifFalse->op){
            auto op = *ifFalse->op;
            if(op == tac::BinaryOperator::EQUALS || op == tac::BinaryOperator::FE){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " == " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(op == tac::BinaryOperator::NOT_EQUALS || op == tac::BinaryOperator::FNE){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " != " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(op == tac::BinaryOperator::LESS || op == tac::BinaryOperator::FL){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " < " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(op == tac::BinaryOperator::LESS_EQUALS || op == tac::BinaryOperator::FLE){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " <= " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(op == tac::BinaryOperator::GREATER || op == tac::BinaryOperator::FG){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " > " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(op == tac::BinaryOperator::GREATER_EQUALS || op == tac::BinaryOperator::FGE){
                std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " >= " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            }
        } else {
            std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " goto " << printTarget(ifFalse) << std::endl;
        }
    }

    void operator()(std::shared_ptr<tac::If>& ifFalse){
        if(ifFalse->op){
            if(*ifFalse->op == tac::BinaryOperator::EQUALS){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " == " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(*ifFalse->op == tac::BinaryOperator::NOT_EQUALS){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " != " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(*ifFalse->op == tac::BinaryOperator::LESS){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " < " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(*ifFalse->op == tac::BinaryOperator::LESS_EQUALS){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " <= " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(*ifFalse->op == tac::BinaryOperator::GREATER){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " > " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            } else if(*ifFalse->op == tac::BinaryOperator::GREATER_EQUALS){
                std::cout << "\tif " << printArgument(ifFalse->arg1) << " >= " << printArgument(*ifFalse->arg2) << " goto " << printTarget(ifFalse) << std::endl;
            }
        } else {
            std::cout << "\tifFalse " << printArgument(ifFalse->arg1) << " goto " << printTarget(ifFalse) << std::endl;
        }
    }

    void operator()(std::shared_ptr<tac::Goto>& goto_){
        std::cout << "\tgoto " << printTarget(goto_) << std::endl;
    }

    void operator()(tac::NoOp&){
        std::cout << "\tno-op" << std::endl;
    }

    void operator()(std::shared_ptr<tac::Call>& call){
        std::cout << "\t";

        if(call->return_){
            std::cout << call->return_->name();
        }

        if(call->return2_){
            std::cout << ", " << call->return2_->name();
        }

        if(call->return_ || call->return2_){
            std::cout << " = ";
        }

        std::cout << "call " << call->function << std::endl;
    }

    void operator()(std::string& label){
        std::cout << "\t" << label << ":" << std::endl;
    }
};

} //end of anonymous namespace

void tac::Printer::print(tac::Program& program) const {
   DebugVisitor visitor;
   visitor(program);
}

void tac::Printer::print(tac::Statement& statement) const {
   DebugVisitor visitor;
   visit(visitor, statement);
}
<|endoftext|>


<commit_before>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2006 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

//$Id: tiff_reader.cpp 17 2005-03-08 23:58:43Z pavlenko $
// stl
#include <iostream>
#include <boost/filesystem/operations.hpp>
// mapnik
#include <mapnik/image_reader.hpp>

extern "C"
{
   #include <tiffio.h>
}

namespace mapnik
{

    using std::min;
    using std::max;

    class TiffReader : public ImageReader
    {
    private:
        std::string file_name_;
        int read_method_;
        unsigned width_;
        unsigned height_;
        int rows_per_strip_;
        int tile_width_;
        int tile_height_;
    public:
        enum {
            generic=1,
            stripped,
            tiled
        };
        explicit TiffReader(const std::string& file_name);
        virtual ~TiffReader();
        unsigned width() const;
        unsigned height() const;
        void read(unsigned x,unsigned y,ImageData32& image);
    private:
        TiffReader(const TiffReader&);
        TiffReader& operator=(const TiffReader&);
        void init();
        void read_generic(unsigned x,unsigned y,ImageData32& image);
        void read_stripped(unsigned x,unsigned y,ImageData32& image);
        void read_tiled(unsigned x,unsigned y,ImageData32& image);
        TIFF* load_if_exists(const std::string& filename);
    };

    namespace
    {
        ImageReader* createTiffReader(const std::string& file)
        {
            return new TiffReader(file);
        }

        const bool registered = register_image_reader("tiff",createTiffReader);
    }

    TiffReader::TiffReader(const std::string& file_name)
        : file_name_(file_name),
          read_method_(generic),
          width_(0),
          height_(0),
          rows_per_strip_(0),
          tile_width_(0),
          tile_height_(0)
    {
        try
        {
            init();
        }
        catch (ImageReaderException& ex)
        {
            std::clog<<ex.what()<<std::endl;
            throw;
        }
    }

    void TiffReader::init()
    {
	TIFF* tif = load_if_exists(file_name_);
        if (!tif) return;

        char msg[1024];

        if (TIFFRGBAImageOK(tif,msg))
        {
            TIFFGetField(tif, TIFFTAG_IMAGEWIDTH, &width_);
            TIFFGetField(tif, TIFFTAG_IMAGELENGTH, &height_);
            if (TIFFIsTiled(tif))
            {
                TIFFGetField(tif, TIFFTAG_TILEWIDTH, &tile_width_);
                TIFFGetField(tif, TIFFTAG_TILELENGTH, &tile_height_);
                read_method_=tiled;
            }
            else if (TIFFGetField(tif,TIFFTAG_ROWSPERSTRIP,&rows_per_strip_)!=0)
            {
                read_method_=stripped;
            }
            TIFFClose(tif);
        }
        else
        {
            TIFFClose(tif);
            throw ImageReaderException(msg);
        }
    }

    TiffReader::~TiffReader()
    {
        //
    }

    unsigned TiffReader::width() const
    {
        return width_;
    }

    unsigned TiffReader::height() const
    {
        return height_;
    }

    void TiffReader::read(unsigned x,unsigned y,ImageData32& image)
    {
        if (read_method_==stripped)
        {
            read_stripped(x,y,image);
        }
        else if (read_method_==tiled)
        {
            read_tiled(x,y,image);
        }
        else
        {
            read_generic(x,y,image);
        }
    }

    void TiffReader::read_generic(unsigned x,unsigned y,ImageData32& image)
    {
	TIFF* tif = load_if_exists(file_name_);
        if (tif)
        {
            std::clog<<"TODO:tiff is not stripped or tiled\n";
            TIFFClose(tif);
        }
    }

    void TiffReader::read_tiled(unsigned x0,unsigned y0,ImageData32& image)
    {
	TIFF* tif = load_if_exists(file_name_);
        if (tif)
        {
            uint32* buf = (uint32*)_TIFFmalloc(tile_width_*tile_height_*sizeof(uint32));
            int width=image.width();
            int height=image.height();

            int start_y=(y0/tile_height_)*tile_height_;
            int end_y=((y0+height)/tile_height_+1)*tile_height_;

            int start_x=(x0/tile_width_)*tile_width_;
            int end_x=((x0+width)/tile_width_+1)*tile_width_;
            int row,tx0,tx1,ty0,ty1;

            for (int y=start_y;y<end_y;y+=tile_height_)
            {
                ty0 = max(y0,(unsigned)y) - y;
                ty1 = min(height+y0,(unsigned)(y+tile_height_)) - y;

                int n0=tile_height_-ty1;
                int n1=tile_height_-ty0-1;

                for (int x=start_x;x<end_x;x+=tile_width_)
                {

                    if (!TIFFReadRGBATile(tif,x,y,buf)) break;

                    tx0=max(x0,(unsigned)x);
                    tx1=min(width+x0,(unsigned)(x+tile_width_));
                    row=y+ty0-y0;
                    for (int n=n1;n>=n0;--n)
                    {
                        image.setRow(row,tx0-x0,tx1-x0,(const unsigned*)&buf[n*tile_width_+tx0-x]);
                        ++row;
                    }
                }
            }
            _TIFFfree(buf);
            TIFFClose(tif);
        }
    }

    void TiffReader::read_stripped(unsigned x0,unsigned y0,ImageData32& image)
    {
	TIFF* tif = load_if_exists(file_name_);
        if (tif)
        {
            uint32* buf = (uint32*)_TIFFmalloc(width_*rows_per_strip_*sizeof(uint32));

            int width=image.width();
            int height=image.height();

            unsigned start_y=(y0/rows_per_strip_)*rows_per_strip_;
            unsigned end_y=((y0+height)/rows_per_strip_+1)*rows_per_strip_;
            bool laststrip=((unsigned)end_y > height_)?true:false;
            int row,tx0,tx1,ty0,ty1;

            tx0=x0;
            tx1=min(width+x0,(unsigned)width_);

            for (unsigned y=start_y; y < end_y; y+=rows_per_strip_)
            {
                ty0 = max(y0,y)-y;
                ty1 = min(height+y0,y+rows_per_strip_)-y;

                if (!TIFFReadRGBAStrip(tif,y,buf)) break;

                row=y+ty0-y0;

                int n0=laststrip ? 0:(rows_per_strip_-ty1);
                int n1=laststrip ? (ty1-ty0-1):(rows_per_strip_-ty0-1);
                for (int n=n1;n>=n0;--n)
                {
                    image.setRow(row,tx0-x0,tx1-x0,(const unsigned*)&buf[n*width_+tx0]);
                    ++row;
                }
            }
            _TIFFfree(buf);
            TIFFClose(tif);
        }
    }

    TIFF* TiffReader::load_if_exists(const std::string& filename)
    {
        TIFF* tif = 0;
        boost::filesystem::path path(file_name_);
        if (exists(path) && is_regular(path)) {
            // File path is a full file path and does exist
            tif = TIFFOpen(filename.c_str(), "rb");
        } else {
            return 0;
        }
        if (!tif) {
            throw ImageReaderException("cannot open "+file_name_);
        }
        return tif;
    }
}

<commit_msg>1. is_regular is not supported in boost 1.33.* 2. cleanups<commit_after>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2006 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

//$Id: tiff_reader.cpp 17 2005-03-08 23:58:43Z pavlenko $
// stl
#include <iostream>
#include <boost/filesystem.hpp>
// mapnik
#include <mapnik/image_reader.hpp>

extern "C"
{
   #include <tiffio.h>
}

namespace mapnik
{

    using std::min;
    using std::max;

    class TiffReader : public ImageReader
    {
    private:
        std::string file_name_;
        int read_method_;
        unsigned width_;
        unsigned height_;
        int rows_per_strip_;
        int tile_width_;
        int tile_height_;
    public:
        enum {
            generic=1,
            stripped,
            tiled
        };
        explicit TiffReader(const std::string& file_name);
        virtual ~TiffReader();
        unsigned width() const;
        unsigned height() const;
        void read(unsigned x,unsigned y,ImageData32& image);
    private:
        TiffReader(const TiffReader&);
        TiffReader& operator=(const TiffReader&);
        void init();
        void read_generic(unsigned x,unsigned y,ImageData32& image);
        void read_stripped(unsigned x,unsigned y,ImageData32& image);
        void read_tiled(unsigned x,unsigned y,ImageData32& image);
        TIFF* load_if_exists(const std::string& filename);
    };

    namespace
    {
        ImageReader* createTiffReader(const std::string& file)
        {
            return new TiffReader(file);
        }

        const bool registered = register_image_reader("tiff",createTiffReader);
    }

    TiffReader::TiffReader(const std::string& file_name)
        : file_name_(file_name),
          read_method_(generic),
          width_(0),
          height_(0),
          rows_per_strip_(0),
          tile_width_(0),
          tile_height_(0)
    {
        try
        {
            init();
        }
        catch (ImageReaderException& ex)
        {
            std::clog<<ex.what()<<std::endl;
            throw;
        }
    }

    void TiffReader::init()
    {
	TIFF* tif = load_if_exists(file_name_);
        if (!tif) return;

        char msg[1024];

        if (TIFFRGBAImageOK(tif,msg))
        {
            TIFFGetField(tif, TIFFTAG_IMAGEWIDTH, &width_);
            TIFFGetField(tif, TIFFTAG_IMAGELENGTH, &height_);
            if (TIFFIsTiled(tif))
            {
                TIFFGetField(tif, TIFFTAG_TILEWIDTH, &tile_width_);
                TIFFGetField(tif, TIFFTAG_TILELENGTH, &tile_height_);
                read_method_=tiled;
            }
            else if (TIFFGetField(tif,TIFFTAG_ROWSPERSTRIP,&rows_per_strip_)!=0)
            {
                read_method_=stripped;
            }
            TIFFClose(tif);
        }
        else
        {
            TIFFClose(tif);
            throw ImageReaderException(msg);
        }
    }

    TiffReader::~TiffReader()
    {
        //
    }

    unsigned TiffReader::width() const
    {
        return width_;
    }

    unsigned TiffReader::height() const
    {
        return height_;
    }

    void TiffReader::read(unsigned x,unsigned y,ImageData32& image)
    {
        if (read_method_==stripped)
        {
            read_stripped(x,y,image);
        }
        else if (read_method_==tiled)
        {
            read_tiled(x,y,image);
        }
        else
        {
            read_generic(x,y,image);
        }
    }

    void TiffReader::read_generic(unsigned x,unsigned y,ImageData32& image)
    {
	TIFF* tif = load_if_exists(file_name_);
        if (tif)
        {
            std::clog<<"TODO:tiff is not stripped or tiled\n";
            TIFFClose(tif);
        }
    }

    void TiffReader::read_tiled(unsigned x0,unsigned y0,ImageData32& image)
    {
	TIFF* tif = load_if_exists(file_name_);
        if (tif)
        {
            uint32* buf = (uint32*)_TIFFmalloc(tile_width_*tile_height_*sizeof(uint32));
            int width=image.width();
            int height=image.height();

            int start_y=(y0/tile_height_)*tile_height_;
            int end_y=((y0+height)/tile_height_+1)*tile_height_;

            int start_x=(x0/tile_width_)*tile_width_;
            int end_x=((x0+width)/tile_width_+1)*tile_width_;
            int row,tx0,tx1,ty0,ty1;

            for (int y=start_y;y<end_y;y+=tile_height_)
            {
                ty0 = max(y0,(unsigned)y) - y;
                ty1 = min(height+y0,(unsigned)(y+tile_height_)) - y;

                int n0=tile_height_-ty1;
                int n1=tile_height_-ty0-1;

                for (int x=start_x;x<end_x;x+=tile_width_)
                {

                    if (!TIFFReadRGBATile(tif,x,y,buf)) break;

                    tx0=max(x0,(unsigned)x);
                    tx1=min(width+x0,(unsigned)(x+tile_width_));
                    row=y+ty0-y0;
                    for (int n=n1;n>=n0;--n)
                    {
                        image.setRow(row,tx0-x0,tx1-x0,(const unsigned*)&buf[n*tile_width_+tx0-x]);
                        ++row;
                    }
                }
            }
            _TIFFfree(buf);
            TIFFClose(tif);
        }
    }

    void TiffReader::read_stripped(unsigned x0,unsigned y0,ImageData32& image)
    {
	TIFF* tif = load_if_exists(file_name_);
        if (tif)
        {
            uint32* buf = (uint32*)_TIFFmalloc(width_*rows_per_strip_*sizeof(uint32));

            int width=image.width();
            int height=image.height();

            unsigned start_y=(y0/rows_per_strip_)*rows_per_strip_;
            unsigned end_y=((y0+height)/rows_per_strip_+1)*rows_per_strip_;
            bool laststrip=((unsigned)end_y > height_)?true:false;
            int row,tx0,tx1,ty0,ty1;

            tx0=x0;
            tx1=min(width+x0,(unsigned)width_);

            for (unsigned y=start_y; y < end_y; y+=rows_per_strip_)
            {
                ty0 = max(y0,y)-y;
                ty1 = min(height+y0,y+rows_per_strip_)-y;

                if (!TIFFReadRGBAStrip(tif,y,buf)) break;

                row=y+ty0-y0;

                int n0=laststrip ? 0:(rows_per_strip_-ty1);
                int n1=laststrip ? (ty1-ty0-1):(rows_per_strip_-ty0-1);
                for (int n=n1;n>=n0;--n)
                {
                    image.setRow(row,tx0-x0,tx1-x0,(const unsigned*)&buf[n*width_+tx0]);
                    ++row;
                }
            }
            _TIFFfree(buf);
            TIFFClose(tif);
        }
    }

    TIFF* TiffReader::load_if_exists(std::string const& filename)
    {
        TIFF * tif = 0;
        boost::filesystem::path path(file_name_);
        if (exists(path)) //  && is_regular(path)) { -- not supported in boost-1.33.*
        {
            // File path is a full file path and does exist
            tif = TIFFOpen(filename.c_str(), "rb");
        }

        return tif;
    }
}

<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file reference_line.cc
 **/

#include "modules/planning/reference_line/reference_line.h"

#include <algorithm>
#include <limits>
#include <string>
#include <utility>
#include <vector>

#include "boost/math/tools/minima.hpp"

#include "modules/common/log.h"
#include "modules/common/math/angle.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/util/string_util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/double.h"

namespace apollo {
namespace planning {

using MapPath = hdmap::Path;
using SLPoint = apollo::common::SLPoint;

ReferenceLine::ReferenceLine(
    const std::vector<ReferencePoint>& reference_points)
    : reference_points_(reference_points),
      map_path_(MapPath(std::vector<hdmap::MapPathPoint>(
          reference_points.begin(), reference_points.end()))) {}

ReferenceLine::ReferenceLine(const MapPath& hdmap_path)
    : map_path_(hdmap_path) {
  for (const auto& point : hdmap_path.path_points()) {
    DCHECK(!point.lane_waypoints().empty());
    const auto& lane_waypoint = point.lane_waypoints()[0];
    reference_points_.emplace_back(
        hdmap::MapPathPoint(point, point.heading(), lane_waypoint), 0.0, 0.0,
        0.0, 0.0);
  }
}

ReferenceLine::ReferenceLine(
    const std::vector<ReferencePoint>& reference_points,
    const std::vector<hdmap::LaneSegment>& lane_segments,
    const double max_approximation_error)
    : reference_points_(reference_points),
      map_path_(MapPath(std::vector<hdmap::MapPathPoint>(
                            reference_points.begin(), reference_points.end()),
                        lane_segments, max_approximation_error)) {}

ReferencePoint ReferenceLine::get_reference_point(const double s) const {
  const auto& accumulated_s = map_path_.accumulated_s();
  if (s < accumulated_s.front()) {
    AWARN << "The requested s is before the start point of the reference "
             "line; reference line starts at "
          << accumulated_s.front() << ", requested " << s << ".";
    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0, 0.0, 0.0);
    if (ref_point.lane_waypoints().empty()) {
      ref_point.add_lane_waypoints(reference_points_.front().lane_waypoints());
    }
    return ref_point;
  }
  if (s > accumulated_s.back()) {
    AWARN << "The requested s exceeds the reference line; reference line "
             "ends at "
          << accumulated_s.back() << "requested " << s << " .";
    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0, 0.0, 0.0);
    if (ref_point.lane_waypoints().empty()) {
      ref_point.add_lane_waypoints(reference_points_.back().lane_waypoints());
    }
    return ref_point;
  }

  auto it_lower =
      std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
  if (it_lower == accumulated_s.begin()) {
    return reference_points_.front();
  } else {
    std::uint32_t index =
        static_cast<std::uint32_t>(it_lower - accumulated_s.begin());
    auto p0 = reference_points_[index - 1];
    auto p1 = reference_points_[index];

    auto s0 = accumulated_s[index - 1];
    auto s1 = accumulated_s[index];

    return interpolate(p0, s0, p1, s1, s);
  }
}

double ReferenceLine::find_min_distance_point(const ReferencePoint& p0,
                                              const double s0,
                                              const ReferencePoint& p1,
                                              const double s1, const double x,
                                              const double y) {
  auto func_dist_square = [&p0, &p1, &s0, &s1, &x, &y](const double s) {
    auto p = interpolate(p0, s0, p1, s1, s);
    double dx = p.x() - x;
    double dy = p.y() - y;
    return dx * dx + dy * dy;
  };

  return ::boost::math::tools::brent_find_minima(func_dist_square, s0, s1, 8)
      .first;
}

ReferencePoint ReferenceLine::get_reference_point(const double x,
                                                  const double y) const {
  CHECK_GE(reference_points_.size(), 0);

  auto func_distance_square = [](const ReferencePoint& point, const double x,
                                 const double y) {
    double dx = point.x() - x;
    double dy = point.y() - y;
    return dx * dx + dy * dy;
  };

  double d_min = func_distance_square(reference_points_.front(), x, y);
  double index_min = 0;

  for (uint32_t i = 1; i < reference_points_.size(); ++i) {
    double d_temp = func_distance_square(reference_points_[i], x, y);
    if (d_temp < d_min) {
      d_min = d_temp;
      index_min = i;
    }
  }

  uint32_t index_start = (index_min == 0 ? index_min : index_min - 1);
  uint32_t index_end =
      (index_min + 1 == reference_points_.size() ? index_min : index_min + 1);

  if (index_start == index_end) {
    return reference_points_[index_start];
  }

  double s0 = map_path_.accumulated_s()[index_start];
  double s1 = map_path_.accumulated_s()[index_end];

  double s = ReferenceLine::find_min_distance_point(
      reference_points_[index_start], s0, reference_points_[index_end], s1, x,
      y);

  return interpolate(reference_points_[index_start], s0,
                     reference_points_[index_end], s1, s);
}

bool ReferenceLine::get_point_in_cartesian_frame(
    const common::SLPoint& sl_point,
    common::math::Vec2d* const xy_point) const {
  CHECK_NOTNULL(xy_point);
  if (map_path_.num_points() < 2) {
    AERROR << "The reference line has too few points.";
    return false;
  }

  const auto matched_point = get_reference_point(sl_point.s());
  const auto angle = common::math::Angle16::from_rad(matched_point.heading());
  xy_point->set_x(matched_point.x() - common::math::sin(angle) * sl_point.l());
  xy_point->set_y(matched_point.y() + common::math::cos(angle) * sl_point.l());
  return true;
}

bool ReferenceLine::get_point_in_frenet_frame(
    const common::math::Vec2d& xy_point,
    common::SLPoint* const sl_point) const {
  DCHECK_NOTNULL(sl_point);
  double s = 0;
  double l = 0;
  if (!map_path_.get_projection(xy_point, &s, &l)) {
    AERROR << "Can't get nearest point from path.";
    return false;
  }

  sl_point->set_s(s);
  sl_point->set_l(l);
  return true;
}

ReferencePoint ReferenceLine::interpolate(const ReferencePoint& p0,
                                          const double s0,
                                          const ReferencePoint& p1,
                                          const double s1, const double s) {
  if (std::fabs(s0 - s1) < common::math::kMathEpsilon) {
    return p0;
  }
  DCHECK_LE(s0, s) << " s: " << s << " is less than s0 :" << s0;
  DCHECK_LE(s, s1) << "s: " << s << "is larger than s1: " << s1;
  CHECK(!p0.lane_waypoints().empty());
  CHECK(!p1.lane_waypoints().empty());
  const double x = common::math::lerp(p0.x(), s0, p1.x(), s1, s);
  const double y = common::math::lerp(p0.y(), s0, p1.y(), s1, s);
  const double heading =
      common::math::slerp(p0.heading(), s0, p1.heading(), s1, s);
  const double kappa = common::math::lerp(p0.kappa(), s0, p1.kappa(), s1, s);
  const double dkappa = common::math::lerp(p0.dkappa(), s0, p1.dkappa(), s1, s);
  const auto& p0_waypoint = p0.lane_waypoints()[0];
  std::vector<hdmap::LaneWaypoint> waypoints;
  double upper_bound = 0.0;
  double lower_bound = 0.0;
  if ((s - s0) + p0_waypoint.s <= p0_waypoint.lane->total_length()) {
    const double lane_s = p0_waypoint.s + s - s0;
    waypoints.emplace_back(p0_waypoint.lane, lane_s);
    p0_waypoint.lane->get_width(lane_s, &upper_bound, &lower_bound);
  }
  const auto& p1_waypoint = p1.lane_waypoints()[0];
  if (p1_waypoint.lane->id().id() != p0_waypoint.lane->id().id() &&
      p1_waypoint.s - (s1 - s) >= 0) {
    const double lane_s = p1_waypoint.s - (s1 - s);
    waypoints.emplace_back(p1_waypoint.lane, lane_s);
    p1_waypoint.lane->get_width(lane_s, &upper_bound, &lower_bound);
  }

  return ReferencePoint(hdmap::MapPathPoint({x, y}, heading, waypoints), kappa,
                        dkappa, lower_bound, upper_bound);
}

const std::vector<ReferencePoint>& ReferenceLine::reference_points() const {
  return reference_points_;
}

const MapPath& ReferenceLine::map_path() const { return map_path_; }

bool ReferenceLine::get_lane_width(const double s, double* const left_width,
                                   double* const right_width) const {
  return map_path_.get_width(s, left_width, right_width);
}

bool ReferenceLine::is_on_road(const common::SLPoint& sl_point) const {
  if (sl_point.s() <= 0 || sl_point.s() > map_path_.length()) {
    return false;
  }
  double left_width = 0.0;
  double right_width = 0.0;

  if (!get_lane_width(sl_point.s(), &left_width, &right_width)) {
    return false;
  }

  if (sl_point.l() <= -right_width || sl_point.l() >= left_width) {
    return false;
  }

  return true;
}

std::string ReferenceLine::DebugString() const {
  const auto limit =
      std::min(reference_points_.size(),
               static_cast<size_t>(FLAGS_trajectory_point_num_for_debug));
  return apollo::common::util::StrCat(
      "point num:", reference_points_.size(),
      apollo::common::util::PrintDebugStringIter(
          reference_points_.begin(), reference_points_.begin() + limit, ""));
}

double ReferenceLine::GetSpeedLimitFromS(const double s) const {
  const auto& map_path_point = get_reference_point(s);
  double speed_limit = FLAGS_planning_speed_upper_limit;
  for (const auto& lane_waypoint : map_path_point.lane_waypoints()) {
    if (lane_waypoint.lane == nullptr) {
      AWARN << "lane_waypoint.lane is nullptr";
      continue;
    }
    speed_limit =
        std::fmin(lane_waypoint.lane->lane().speed_limit(), speed_limit);
  }
  return speed_limit;
}

double ReferenceLine::GetSpeedLimitFromPoint(
    const common::math::Vec2d& point) const {
  SLPoint sl;
  if (!get_point_in_frenet_frame(point, &sl)) {
    AWARN << "Failed to get projection for point: " << point.DebugString();
    return FLAGS_planning_speed_upper_limit;
  }
  return GetSpeedLimitFromS(sl.s());
}

}  // namespace planning
}  // namespace apollo
<commit_msg>planning: added a safety margin for s boundary checking in reference line interpolation<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file reference_line.cc
 **/

#include "modules/planning/reference_line/reference_line.h"

#include <algorithm>
#include <limits>
#include <string>
#include <utility>
#include <vector>

#include "boost/math/tools/minima.hpp"

#include "modules/common/log.h"
#include "modules/common/math/angle.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/util/string_util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/double.h"

namespace apollo {
namespace planning {

using MapPath = hdmap::Path;
using SLPoint = apollo::common::SLPoint;

ReferenceLine::ReferenceLine(
    const std::vector<ReferencePoint>& reference_points)
    : reference_points_(reference_points),
      map_path_(MapPath(std::vector<hdmap::MapPathPoint>(
          reference_points.begin(), reference_points.end()))) {}

ReferenceLine::ReferenceLine(const MapPath& hdmap_path)
    : map_path_(hdmap_path) {
  for (const auto& point : hdmap_path.path_points()) {
    DCHECK(!point.lane_waypoints().empty());
    const auto& lane_waypoint = point.lane_waypoints()[0];
    reference_points_.emplace_back(
        hdmap::MapPathPoint(point, point.heading(), lane_waypoint), 0.0, 0.0,
        0.0, 0.0);
  }
}

ReferenceLine::ReferenceLine(
    const std::vector<ReferencePoint>& reference_points,
    const std::vector<hdmap::LaneSegment>& lane_segments,
    const double max_approximation_error)
    : reference_points_(reference_points),
      map_path_(MapPath(std::vector<hdmap::MapPathPoint>(
                            reference_points.begin(), reference_points.end()),
                        lane_segments, max_approximation_error)) {}

ReferencePoint ReferenceLine::get_reference_point(const double s) const {
  const auto& accumulated_s = map_path_.accumulated_s();
  if (s < accumulated_s.front()) {
    AWARN << "The requested s is before the start point of the reference "
             "line; reference line starts at "
          << accumulated_s.front() << ", requested " << s << ".";
    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0, 0.0, 0.0);
    if (ref_point.lane_waypoints().empty()) {
      ref_point.add_lane_waypoints(reference_points_.front().lane_waypoints());
    }
    return ref_point;
  }
  if (s > accumulated_s.back()) {
    AWARN << "The requested s exceeds the reference line; reference line "
             "ends at "
          << accumulated_s.back() << "requested " << s << " .";
    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0, 0.0, 0.0);
    if (ref_point.lane_waypoints().empty()) {
      ref_point.add_lane_waypoints(reference_points_.back().lane_waypoints());
    }
    return ref_point;
  }

  auto it_lower =
      std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
  if (it_lower == accumulated_s.begin()) {
    return reference_points_.front();
  } else {
    std::uint32_t index =
        static_cast<std::uint32_t>(it_lower - accumulated_s.begin());
    auto p0 = reference_points_[index - 1];
    auto p1 = reference_points_[index];

    auto s0 = accumulated_s[index - 1];
    auto s1 = accumulated_s[index];

    return interpolate(p0, s0, p1, s1, s);
  }
}

double ReferenceLine::find_min_distance_point(const ReferencePoint& p0,
                                              const double s0,
                                              const ReferencePoint& p1,
                                              const double s1, const double x,
                                              const double y) {
  auto func_dist_square = [&p0, &p1, &s0, &s1, &x, &y](const double s) {
    auto p = interpolate(p0, s0, p1, s1, s);
    double dx = p.x() - x;
    double dy = p.y() - y;
    return dx * dx + dy * dy;
  };

  return ::boost::math::tools::brent_find_minima(func_dist_square, s0, s1, 8)
      .first;
}

ReferencePoint ReferenceLine::get_reference_point(const double x,
                                                  const double y) const {
  CHECK_GE(reference_points_.size(), 0);

  auto func_distance_square = [](const ReferencePoint& point, const double x,
                                 const double y) {
    double dx = point.x() - x;
    double dy = point.y() - y;
    return dx * dx + dy * dy;
  };

  double d_min = func_distance_square(reference_points_.front(), x, y);
  double index_min = 0;

  for (uint32_t i = 1; i < reference_points_.size(); ++i) {
    double d_temp = func_distance_square(reference_points_[i], x, y);
    if (d_temp < d_min) {
      d_min = d_temp;
      index_min = i;
    }
  }

  uint32_t index_start = (index_min == 0 ? index_min : index_min - 1);
  uint32_t index_end =
      (index_min + 1 == reference_points_.size() ? index_min : index_min + 1);

  if (index_start == index_end) {
    return reference_points_[index_start];
  }

  double s0 = map_path_.accumulated_s()[index_start];
  double s1 = map_path_.accumulated_s()[index_end];

  double s = ReferenceLine::find_min_distance_point(
      reference_points_[index_start], s0, reference_points_[index_end], s1, x,
      y);

  return interpolate(reference_points_[index_start], s0,
                     reference_points_[index_end], s1, s);
}

bool ReferenceLine::get_point_in_cartesian_frame(
    const common::SLPoint& sl_point,
    common::math::Vec2d* const xy_point) const {
  CHECK_NOTNULL(xy_point);
  if (map_path_.num_points() < 2) {
    AERROR << "The reference line has too few points.";
    return false;
  }

  const auto matched_point = get_reference_point(sl_point.s());
  const auto angle = common::math::Angle16::from_rad(matched_point.heading());
  xy_point->set_x(matched_point.x() - common::math::sin(angle) * sl_point.l());
  xy_point->set_y(matched_point.y() + common::math::cos(angle) * sl_point.l());
  return true;
}

bool ReferenceLine::get_point_in_frenet_frame(
    const common::math::Vec2d& xy_point,
    common::SLPoint* const sl_point) const {
  DCHECK_NOTNULL(sl_point);
  double s = 0;
  double l = 0;
  if (!map_path_.get_projection(xy_point, &s, &l)) {
    AERROR << "Can't get nearest point from path.";
    return false;
  }

  sl_point->set_s(s);
  sl_point->set_l(l);
  return true;
}

ReferencePoint ReferenceLine::interpolate(const ReferencePoint& p0,
                                          const double s0,
                                          const ReferencePoint& p1,
                                          const double s1, const double s) {
  if (std::fabs(s0 - s1) < common::math::kMathEpsilon) {
    return p0;
  }
  DCHECK_LE(s0 - 1.0e-6, s) << " s: " << s << " is less than s0 :" << s0;
  DCHECK_LE(s, s1 + 1.0e-6) << "s: " << s << "is larger than s1: " << s1;

  CHECK(!p0.lane_waypoints().empty());
  CHECK(!p1.lane_waypoints().empty());
  const double x = common::math::lerp(p0.x(), s0, p1.x(), s1, s);
  const double y = common::math::lerp(p0.y(), s0, p1.y(), s1, s);
  const double heading =
      common::math::slerp(p0.heading(), s0, p1.heading(), s1, s);
  const double kappa = common::math::lerp(p0.kappa(), s0, p1.kappa(), s1, s);
  const double dkappa = common::math::lerp(p0.dkappa(), s0, p1.dkappa(), s1, s);
  const auto& p0_waypoint = p0.lane_waypoints()[0];
  std::vector<hdmap::LaneWaypoint> waypoints;
  double upper_bound = 0.0;
  double lower_bound = 0.0;
  if ((s - s0) + p0_waypoint.s <= p0_waypoint.lane->total_length()) {
    const double lane_s = p0_waypoint.s + s - s0;
    waypoints.emplace_back(p0_waypoint.lane, lane_s);
    p0_waypoint.lane->get_width(lane_s, &upper_bound, &lower_bound);
  }
  const auto& p1_waypoint = p1.lane_waypoints()[0];
  if (p1_waypoint.lane->id().id() != p0_waypoint.lane->id().id() &&
      p1_waypoint.s - (s1 - s) >= 0) {
    const double lane_s = p1_waypoint.s - (s1 - s);
    waypoints.emplace_back(p1_waypoint.lane, lane_s);
    p1_waypoint.lane->get_width(lane_s, &upper_bound, &lower_bound);
  }

  return ReferencePoint(hdmap::MapPathPoint({x, y}, heading, waypoints), kappa,
                        dkappa, lower_bound, upper_bound);
}

const std::vector<ReferencePoint>& ReferenceLine::reference_points() const {
  return reference_points_;
}

const MapPath& ReferenceLine::map_path() const { return map_path_; }

bool ReferenceLine::get_lane_width(const double s, double* const left_width,
                                   double* const right_width) const {
  return map_path_.get_width(s, left_width, right_width);
}

bool ReferenceLine::is_on_road(const common::SLPoint& sl_point) const {
  if (sl_point.s() <= 0 || sl_point.s() > map_path_.length()) {
    return false;
  }
  double left_width = 0.0;
  double right_width = 0.0;

  if (!get_lane_width(sl_point.s(), &left_width, &right_width)) {
    return false;
  }

  if (sl_point.l() <= -right_width || sl_point.l() >= left_width) {
    return false;
  }

  return true;
}

std::string ReferenceLine::DebugString() const {
  const auto limit =
      std::min(reference_points_.size(),
               static_cast<size_t>(FLAGS_trajectory_point_num_for_debug));
  return apollo::common::util::StrCat(
      "point num:", reference_points_.size(),
      apollo::common::util::PrintDebugStringIter(
          reference_points_.begin(), reference_points_.begin() + limit, ""));
}

double ReferenceLine::GetSpeedLimitFromS(const double s) const {
  const auto& map_path_point = get_reference_point(s);
  double speed_limit = FLAGS_planning_speed_upper_limit;
  for (const auto& lane_waypoint : map_path_point.lane_waypoints()) {
    if (lane_waypoint.lane == nullptr) {
      AWARN << "lane_waypoint.lane is nullptr";
      continue;
    }
    speed_limit =
        std::fmin(lane_waypoint.lane->lane().speed_limit(), speed_limit);
  }
  return speed_limit;
}

double ReferenceLine::GetSpeedLimitFromPoint(
    const common::math::Vec2d& point) const {
  SLPoint sl;
  if (!get_point_in_frenet_frame(point, &sl)) {
    AWARN << "Failed to get projection for point: " << point.DebugString();
    return FLAGS_planning_speed_upper_limit;
  }
  return GetSpeedLimitFromS(sl.s());
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/prediction/predictor/predictor_manager.h"

#include <memory>

#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/container/adc_trajectory/adc_trajectory_container.h"
#include "modules/prediction/container/container_manager.h"
#include "modules/prediction/container/obstacles/obstacles_container.h"
#include "modules/prediction/predictor/free_move/free_move_predictor.h"
#include "modules/prediction/predictor/lane_sequence/lane_sequence_predictor.h"
#include "modules/prediction/predictor/move_sequence/move_sequence_predictor.h"
#include "modules/prediction/predictor/regional/regional_predictor.h"

namespace apollo {
namespace prediction {

using apollo::common::adapter::AdapterConfig;
using apollo::perception::PerceptionObstacle;
using apollo::perception::PerceptionObstacles;
using apollo::planning::ADCTrajectory;

PredictorManager::PredictorManager() { RegisterPredictors(); }

void PredictorManager::RegisterPredictors() {
  RegisterPredictor(ObstacleConf::LANE_SEQUENCE_PREDICTOR);
  RegisterPredictor(ObstacleConf::MOVE_SEQUENCE_PREDICTOR);
  RegisterPredictor(ObstacleConf::FREE_MOVE_PREDICTOR);
  RegisterPredictor(ObstacleConf::REGIONAL_PREDICTOR);
}

void PredictorManager::Init(const PredictionConf& config) {
  for (const auto& obstacle_conf : config.obstacle_conf()) {
    if (!obstacle_conf.has_obstacle_type()) {
      AERROR << "Obstacle config [" << obstacle_conf.ShortDebugString()
             << "] has not defined obstacle type.";
      continue;
    }

    if (!obstacle_conf.has_predictor_type()) {
      AERROR << "Obstacle config [" << obstacle_conf.ShortDebugString()
             << "] has not defined predictor type.";
      continue;
    }

    switch (obstacle_conf.obstacle_type()) {
      case PerceptionObstacle::VEHICLE: {
        if (obstacle_conf.has_obstacle_status()) {
          if (obstacle_conf.obstacle_status() == ObstacleConf::ON_LANE) {
            vehicle_on_lane_predictor_ = obstacle_conf.predictor_type();
          } else if (obstacle_conf.obstacle_status() ==
                     ObstacleConf::OFF_LANE) {
            vehicle_off_lane_predictor_ = obstacle_conf.predictor_type();
          }
        }
        break;
      }
      case PerceptionObstacle::BICYCLE: {
        if (obstacle_conf.has_obstacle_status()) {
          if (obstacle_conf.obstacle_status() == ObstacleConf::ON_LANE) {
            cyclist_on_lane_predictor_ = obstacle_conf.predictor_type();
          } else if (obstacle_conf.obstacle_status() ==
                     ObstacleConf::OFF_LANE) {
            cyclist_off_lane_predictor_ = obstacle_conf.predictor_type();
          }
        }
        break;
      }
      case PerceptionObstacle::PEDESTRIAN: {
        pedestrian_predictor_ = obstacle_conf.predictor_type();
        break;
      }
      case PerceptionObstacle::UNKNOWN: {
        if (obstacle_conf.has_obstacle_status()) {
          if (obstacle_conf.obstacle_status() == ObstacleConf::ON_LANE) {
            default_on_lane_predictor_ = obstacle_conf.predictor_type();
          } else if (obstacle_conf.obstacle_status() ==
                     ObstacleConf::OFF_LANE) {
            default_off_lane_predictor_ = obstacle_conf.predictor_type();
          }
        }
        break;
      }
      default: { break; }
    }
  }

  AINFO << "Defined vehicle on lane obstacle predictor ["
        << vehicle_on_lane_predictor_ << "].";
  AINFO << "Defined vehicle off lane obstacle predictor ["
        << vehicle_off_lane_predictor_ << "].";
  AINFO << "Defined bicycle on lane obstacle predictor ["
        << cyclist_on_lane_predictor_ << "].";
  AINFO << "Defined bicycle off lane obstacle predictor ["
        << cyclist_off_lane_predictor_ << "].";
  AINFO << "Defined pedestrian obstacle predictor ["
        << pedestrian_predictor_ << "].";
  AINFO << "Defined default on lane obstacle predictor ["
        << default_on_lane_predictor_ << "].";
  AINFO << "Defined default off lane obstacle predictor ["
        << default_off_lane_predictor_ << "].";
}

Predictor* PredictorManager::GetPredictor(
    const ObstacleConf::PredictorType& type) {
  auto it = predictors_.find(type);
  return it != predictors_.end() ? it->second.get() : nullptr;
}

void PredictorManager::Run(const PerceptionObstacles& perception_obstacles) {
  prediction_obstacles_.Clear();
  ObstaclesContainer* obstacles_container = dynamic_cast<ObstaclesContainer*>(
      ContainerManager::instance()->GetContainer(
          AdapterConfig::PERCEPTION_OBSTACLES));
  ADCTrajectoryContainer* adc_trajectory_container =
      dynamic_cast<ADCTrajectoryContainer*>(
          ContainerManager::instance()->GetContainer(
              AdapterConfig::PLANNING_TRAJECTORY));

  CHECK_NOTNULL(obstacles_container);
  CHECK_NOTNULL(adc_trajectory_container);

  Predictor* predictor = nullptr;
  for (const auto& perception_obstacle :
       perception_obstacles.perception_obstacle()) {
    if (!perception_obstacle.has_id()) {
      AERROR << "A perception obstacle has no id.";
      continue;
    }

    int id = perception_obstacle.id();
    if (id < 0) {
      AERROR << "A perception obstacle has invalid id [" << id << "].";
      continue;
    }
    if (perception_obstacle.confidence() <
        FLAGS_perception_confidence_threshold) {
      AWARN << "Skip low confidence obstacle:"
            << perception_obstacle.ShortDebugString();
      continue;
    }

    PredictionObstacle prediction_obstacle;
    prediction_obstacle.set_timestamp(perception_obstacle.timestamp());
    Obstacle* obstacle = obstacles_container->GetObstacle(id);
    if (obstacle != nullptr) {
      switch (perception_obstacle.type()) {
        case PerceptionObstacle::VEHICLE: {
          if (obstacle->IsOnLane()) {
            predictor = GetPredictor(vehicle_on_lane_predictor_);
          } else {
            predictor = GetPredictor(vehicle_off_lane_predictor_);
          }
          break;
        }
        case PerceptionObstacle::PEDESTRIAN: {
          predictor = GetPredictor(pedestrian_predictor_);
          break;
        }
        case PerceptionObstacle::BICYCLE: {
          if (obstacle->IsOnLane() && !obstacle->IsNearJunction()) {
            predictor = GetPredictor(cyclist_on_lane_predictor_);
          } else {
            predictor = GetPredictor(cyclist_off_lane_predictor_);
          }
          break;
        }
        default: {
          if (obstacle->IsOnLane()) {
            predictor = GetPredictor(default_on_lane_predictor_);
          } else {
            predictor = GetPredictor(default_off_lane_predictor_);
          }
          break;
        }
      }

      if (predictor != nullptr) {
        predictor->Predict(obstacle);
        if (FLAGS_enable_trim_prediction_trajectory) {
          predictor->TrimTrajectories(obstacle, adc_trajectory_container);
        }
        for (const auto& trajectory : predictor->trajectories()) {
          prediction_obstacle.add_trajectory()->CopyFrom(trajectory);
        }
      }
      prediction_obstacle.set_timestamp(obstacle->timestamp());
    }

    prediction_obstacle.set_predicted_period(FLAGS_prediction_duration);
    prediction_obstacle.mutable_perception_obstacle()->CopyFrom(
        perception_obstacle);

    prediction_obstacles_.add_prediction_obstacle()->CopyFrom(
        prediction_obstacle);
  }
  prediction_obstacles_.set_perception_error_code(
      perception_obstacles.error_code());
}

std::unique_ptr<Predictor> PredictorManager::CreatePredictor(
    const ObstacleConf::PredictorType& type) {
  std::unique_ptr<Predictor> predictor_ptr(nullptr);
  switch (type) {
    case ObstacleConf::LANE_SEQUENCE_PREDICTOR: {
      predictor_ptr.reset(new LaneSequencePredictor());
      break;
    }
    case ObstacleConf::MOVE_SEQUENCE_PREDICTOR: {
      predictor_ptr.reset(new MoveSequencePredictor());
      break;
    }
    case ObstacleConf::FREE_MOVE_PREDICTOR: {
      predictor_ptr.reset(new FreeMovePredictor());
      break;
    }
    case ObstacleConf::REGIONAL_PREDICTOR: {
      predictor_ptr.reset(new RegionalPredictor());
      break;
    }
    default: { break; }
  }
  return predictor_ptr;
}

void PredictorManager::RegisterPredictor(
    const ObstacleConf::PredictorType& type) {
  predictors_[type] = CreatePredictor(type);
  AINFO << "Predictor [" << type << "] is registered.";
}

const PredictionObstacles& PredictorManager::prediction_obstacles() {
  return prediction_obstacles_;
}

}  // namespace prediction
}  // namespace apollo
<commit_msg>Prediction: Only trim vehicle trajectories in junction<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/prediction/predictor/predictor_manager.h"

#include <memory>

#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/container/adc_trajectory/adc_trajectory_container.h"
#include "modules/prediction/container/container_manager.h"
#include "modules/prediction/container/obstacles/obstacles_container.h"
#include "modules/prediction/predictor/free_move/free_move_predictor.h"
#include "modules/prediction/predictor/lane_sequence/lane_sequence_predictor.h"
#include "modules/prediction/predictor/move_sequence/move_sequence_predictor.h"
#include "modules/prediction/predictor/regional/regional_predictor.h"

namespace apollo {
namespace prediction {

using apollo::common::adapter::AdapterConfig;
using apollo::perception::PerceptionObstacle;
using apollo::perception::PerceptionObstacles;
using apollo::planning::ADCTrajectory;

PredictorManager::PredictorManager() { RegisterPredictors(); }

void PredictorManager::RegisterPredictors() {
  RegisterPredictor(ObstacleConf::LANE_SEQUENCE_PREDICTOR);
  RegisterPredictor(ObstacleConf::MOVE_SEQUENCE_PREDICTOR);
  RegisterPredictor(ObstacleConf::FREE_MOVE_PREDICTOR);
  RegisterPredictor(ObstacleConf::REGIONAL_PREDICTOR);
}

void PredictorManager::Init(const PredictionConf& config) {
  for (const auto& obstacle_conf : config.obstacle_conf()) {
    if (!obstacle_conf.has_obstacle_type()) {
      AERROR << "Obstacle config [" << obstacle_conf.ShortDebugString()
             << "] has not defined obstacle type.";
      continue;
    }

    if (!obstacle_conf.has_predictor_type()) {
      AERROR << "Obstacle config [" << obstacle_conf.ShortDebugString()
             << "] has not defined predictor type.";
      continue;
    }

    switch (obstacle_conf.obstacle_type()) {
      case PerceptionObstacle::VEHICLE: {
        if (obstacle_conf.has_obstacle_status()) {
          if (obstacle_conf.obstacle_status() == ObstacleConf::ON_LANE) {
            vehicle_on_lane_predictor_ = obstacle_conf.predictor_type();
          } else if (obstacle_conf.obstacle_status() ==
                     ObstacleConf::OFF_LANE) {
            vehicle_off_lane_predictor_ = obstacle_conf.predictor_type();
          }
        }
        break;
      }
      case PerceptionObstacle::BICYCLE: {
        if (obstacle_conf.has_obstacle_status()) {
          if (obstacle_conf.obstacle_status() == ObstacleConf::ON_LANE) {
            cyclist_on_lane_predictor_ = obstacle_conf.predictor_type();
          } else if (obstacle_conf.obstacle_status() ==
                     ObstacleConf::OFF_LANE) {
            cyclist_off_lane_predictor_ = obstacle_conf.predictor_type();
          }
        }
        break;
      }
      case PerceptionObstacle::PEDESTRIAN: {
        pedestrian_predictor_ = obstacle_conf.predictor_type();
        break;
      }
      case PerceptionObstacle::UNKNOWN: {
        if (obstacle_conf.has_obstacle_status()) {
          if (obstacle_conf.obstacle_status() == ObstacleConf::ON_LANE) {
            default_on_lane_predictor_ = obstacle_conf.predictor_type();
          } else if (obstacle_conf.obstacle_status() ==
                     ObstacleConf::OFF_LANE) {
            default_off_lane_predictor_ = obstacle_conf.predictor_type();
          }
        }
        break;
      }
      default: { break; }
    }
  }

  AINFO << "Defined vehicle on lane obstacle predictor ["
        << vehicle_on_lane_predictor_ << "].";
  AINFO << "Defined vehicle off lane obstacle predictor ["
        << vehicle_off_lane_predictor_ << "].";
  AINFO << "Defined bicycle on lane obstacle predictor ["
        << cyclist_on_lane_predictor_ << "].";
  AINFO << "Defined bicycle off lane obstacle predictor ["
        << cyclist_off_lane_predictor_ << "].";
  AINFO << "Defined pedestrian obstacle predictor ["
        << pedestrian_predictor_ << "].";
  AINFO << "Defined default on lane obstacle predictor ["
        << default_on_lane_predictor_ << "].";
  AINFO << "Defined default off lane obstacle predictor ["
        << default_off_lane_predictor_ << "].";
}

Predictor* PredictorManager::GetPredictor(
    const ObstacleConf::PredictorType& type) {
  auto it = predictors_.find(type);
  return it != predictors_.end() ? it->second.get() : nullptr;
}

void PredictorManager::Run(const PerceptionObstacles& perception_obstacles) {
  prediction_obstacles_.Clear();
  ObstaclesContainer* obstacles_container = dynamic_cast<ObstaclesContainer*>(
      ContainerManager::instance()->GetContainer(
          AdapterConfig::PERCEPTION_OBSTACLES));
  ADCTrajectoryContainer* adc_trajectory_container =
      dynamic_cast<ADCTrajectoryContainer*>(
          ContainerManager::instance()->GetContainer(
              AdapterConfig::PLANNING_TRAJECTORY));

  CHECK_NOTNULL(obstacles_container);
  CHECK_NOTNULL(adc_trajectory_container);

  Predictor* predictor = nullptr;
  for (const auto& perception_obstacle :
       perception_obstacles.perception_obstacle()) {
    if (!perception_obstacle.has_id()) {
      AERROR << "A perception obstacle has no id.";
      continue;
    }

    int id = perception_obstacle.id();
    if (id < 0) {
      AERROR << "A perception obstacle has invalid id [" << id << "].";
      continue;
    }
    if (perception_obstacle.confidence() <
        FLAGS_perception_confidence_threshold) {
      AWARN << "Skip low confidence obstacle:"
            << perception_obstacle.ShortDebugString();
      continue;
    }

    PredictionObstacle prediction_obstacle;
    prediction_obstacle.set_timestamp(perception_obstacle.timestamp());
    Obstacle* obstacle = obstacles_container->GetObstacle(id);
    if (obstacle != nullptr) {
      switch (perception_obstacle.type()) {
        case PerceptionObstacle::VEHICLE: {
          if (obstacle->IsOnLane()) {
            predictor = GetPredictor(vehicle_on_lane_predictor_);
          } else {
            predictor = GetPredictor(vehicle_off_lane_predictor_);
          }
          break;
        }
        case PerceptionObstacle::PEDESTRIAN: {
          predictor = GetPredictor(pedestrian_predictor_);
          break;
        }
        case PerceptionObstacle::BICYCLE: {
          if (obstacle->IsOnLane() && !obstacle->IsNearJunction()) {
            predictor = GetPredictor(cyclist_on_lane_predictor_);
          } else {
            predictor = GetPredictor(cyclist_off_lane_predictor_);
          }
          break;
        }
        default: {
          if (obstacle->IsOnLane()) {
            predictor = GetPredictor(default_on_lane_predictor_);
          } else {
            predictor = GetPredictor(default_off_lane_predictor_);
          }
          break;
        }
      }

      if (predictor != nullptr) {
        predictor->Predict(obstacle);
        if (FLAGS_enable_trim_prediction_trajectory &&
            obstacle->type() == PerceptionObstacle::VEHICLE) {
          predictor->TrimTrajectories(obstacle, adc_trajectory_container);
        }
        for (const auto& trajectory : predictor->trajectories()) {
          prediction_obstacle.add_trajectory()->CopyFrom(trajectory);
        }
      }
      prediction_obstacle.set_timestamp(obstacle->timestamp());
    }

    prediction_obstacle.set_predicted_period(FLAGS_prediction_duration);
    prediction_obstacle.mutable_perception_obstacle()->CopyFrom(
        perception_obstacle);

    prediction_obstacles_.add_prediction_obstacle()->CopyFrom(
        prediction_obstacle);
  }
  prediction_obstacles_.set_perception_error_code(
      perception_obstacles.error_code());
}

std::unique_ptr<Predictor> PredictorManager::CreatePredictor(
    const ObstacleConf::PredictorType& type) {
  std::unique_ptr<Predictor> predictor_ptr(nullptr);
  switch (type) {
    case ObstacleConf::LANE_SEQUENCE_PREDICTOR: {
      predictor_ptr.reset(new LaneSequencePredictor());
      break;
    }
    case ObstacleConf::MOVE_SEQUENCE_PREDICTOR: {
      predictor_ptr.reset(new MoveSequencePredictor());
      break;
    }
    case ObstacleConf::FREE_MOVE_PREDICTOR: {
      predictor_ptr.reset(new FreeMovePredictor());
      break;
    }
    case ObstacleConf::REGIONAL_PREDICTOR: {
      predictor_ptr.reset(new RegionalPredictor());
      break;
    }
    default: { break; }
  }
  return predictor_ptr;
}

void PredictorManager::RegisterPredictor(
    const ObstacleConf::PredictorType& type) {
  predictors_[type] = CreatePredictor(type);
  AINFO << "Predictor [" << type << "] is registered.";
}

const PredictionObstacles& PredictorManager::prediction_obstacles() {
  return prediction_obstacles_;
}

}  // namespace prediction
}  // namespace apollo
<|endoftext|>


<commit_before>/**
 * \file timeout.cc
 *
 *
 *
 * \author Ethan Burns
 * \date 2008-12-16
 */

#include <iostream>

#include <string.h>
#include <signal.h>
#include <stdlib.h>
#include <unistd.h>

#include "../search.h"

using namespace std;

static volatile bool timeout_stopped = false;

static unsigned int timelimit = 0;

extern "C" void alarm_action(int sig)
{
	if (timeout_stopped)
		return;

	output_search_stats_on_timeout();

	cout << "# Time out" << endl
	     << "time-limit: " << timelimit << endl
	     << "cost: infinity" << endl
	     << "length: infinity" << endl
	     << "wall_time: infinity" << endl
	     << "CPU_time: infinity" << endl
	     << "generated: infinity" << endl
	     << "expanded: infinity" << endl;

	exit(EXIT_SUCCESS);
}

void timeout(unsigned int sec)
{
	struct sigaction sa;

	timelimit = sec;

	memset(&sa, '\0', sizeof(sa));
	sa.sa_handler = alarm_action;
	sigfillset(&sa.sa_mask);
	sigaction(SIGALRM, &sa, NULL);

	timeout_stopped = false;
	alarm(sec);
}

void timeout_stop(void)
{
	timeout_stopped = true;
}
<commit_msg>Call _exit() instead of exit() in the timeout signal handler.<commit_after>/**
 * \file timeout.cc
 *
 *
 *
 * \author Ethan Burns
 * \date 2008-12-16
 */

#include <iostream>

#include <string.h>
#include <signal.h>
#include <stdlib.h>
#include <unistd.h>

#include "../search.h"

using namespace std;

static volatile bool timeout_stopped = false;

static unsigned int timelimit = 0;

extern "C" void alarm_action(int sig)
{
	if (timeout_stopped)
		return;

	output_search_stats_on_timeout();

	cout << "# Time out" << endl
	     << "time-limit: " << timelimit << endl
	     << "cost: infinity" << endl
	     << "length: infinity" << endl
	     << "wall_time: infinity" << endl
	     << "CPU_time: infinity" << endl
	     << "generated: infinity" << endl
	     << "expanded: infinity" << endl;

	_exit(EXIT_SUCCESS);
}

void timeout(unsigned int sec)
{
	struct sigaction sa;

	timelimit = sec;

	memset(&sa, '\0', sizeof(sa));
	sa.sa_handler = alarm_action;
	sigfillset(&sa.sa_mask);
	sigaction(SIGALRM, &sa, NULL);

	timeout_stopped = false;
	alarm(sec);
}

void timeout_stop(void)
{
	timeout_stopped = true;
}
<|endoftext|>


<commit_before>// MIT License

// Copyright (c) 2017 Zhuhao Wang

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "nekit/utils/domain.h"

#include "nekit/utils/error.h"
#include "nekit/utils/log.h"
#include "nekit/utils/runtime.h"

#undef NECHANNEL
#define NECHANNEL "Domain"

namespace nekit {
namespace utils {
Domain::Domain(std::string domain) : domain_{domain} {}

bool Domain::operator==(const std::string& rhs) const { return domain_ == rhs; }

void Domain::Resolve(EventHandler&& handler) {
  if (resolved_) {
    NEDEBUG << "Already resolved, does nothing.";
    Runtime::CurrentRuntime().IoService()->post([handler{
        std::move(handler)}]() { handler(NEKitErrorCode::NoError); });
    return;
  }

  ForceResolve(std::move(handler));
}

void Domain::ForceResolve(EventHandler&& handler) {
  assert(!resolving_);

  NEDEBUG << "Start resolving domain " << domain_ << "now.";

  resolving_ = true;
  Runtime::CurrentRuntime().Resolver()->Resolve(
      domain_, ResolverInterface::AddressPreference::Any,
      [ this, handler{std::move(handler)} ](
          std::shared_ptr<std::vector<boost::asio::ip::address>> addresses,
          std::error_code ec) {

        resolving_ = false;
        resolved_ = true;

        if (ec) {
          NEERROR << "Failed to resolve " << domain_ << " due to " << ec << ".";
          error_ = ec;
          addresses_ = nullptr;
          handler(ec);
          return;
        }

        NEINFO << "Successfully resolved domain " << domain_ << ".";

        addresses_ = addresses;
        handler(ec);
      });
}

bool Domain::isResolved() const { return resolved_; }

bool Domain::isResolving() const { return resolving_; }

bool Domain::isFailed() const { return resolved_ && addresses_; }

bool Domain::isAddressAvailable() const {
  return addresses_ && !addresses_->empty();
}

std::error_code Domain::error() const { return error_; }

std::shared_ptr<const std::vector<boost::asio::ip::address>> Domain::addresses()
    const {
  return addresses_;
}

}  // namespace utils
}  // namespace nekit
<commit_msg>FIX: Fix log format<commit_after>// MIT License

// Copyright (c) 2017 Zhuhao Wang

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "nekit/utils/domain.h"

#include "nekit/utils/error.h"
#include "nekit/utils/log.h"
#include "nekit/utils/runtime.h"

#undef NECHANNEL
#define NECHANNEL "Domain"

namespace nekit {
namespace utils {
Domain::Domain(std::string domain) : domain_{domain} {}

bool Domain::operator==(const std::string& rhs) const { return domain_ == rhs; }

void Domain::Resolve(EventHandler&& handler) {
  if (resolved_) {
    NEDEBUG << "Already resolved, does nothing.";
    Runtime::CurrentRuntime().IoService()->post([handler{
        std::move(handler)}]() { handler(NEKitErrorCode::NoError); });
    return;
  }

  ForceResolve(std::move(handler));
}

void Domain::ForceResolve(EventHandler&& handler) {
  assert(!resolving_);

  NEDEBUG << "Start resolving domain " << domain_ << ".";

  resolving_ = true;
  Runtime::CurrentRuntime().Resolver()->Resolve(
      domain_, ResolverInterface::AddressPreference::Any,
      [ this, handler{std::move(handler)} ](
          std::shared_ptr<std::vector<boost::asio::ip::address>> addresses,
          std::error_code ec) {

        resolving_ = false;
        resolved_ = true;

        if (ec) {
          NEERROR << "Failed to resolve " << domain_ << " due to " << ec << ".";
          error_ = ec;
          addresses_ = nullptr;
          handler(ec);
          return;
        }

        NEINFO << "Successfully resolved domain " << domain_ << ".";

        addresses_ = addresses;
        handler(ec);
      });
}

bool Domain::isResolved() const { return resolved_; }

bool Domain::isResolving() const { return resolving_; }

bool Domain::isFailed() const { return resolved_ && addresses_; }

bool Domain::isAddressAvailable() const {
  return addresses_ && !addresses_->empty();
}

std::error_code Domain::error() const { return error_; }

std::shared_ptr<const std::vector<boost::asio::ip::address>> Domain::addresses()
    const {
  return addresses_;
}

}  // namespace utils
}  // namespace nekit
<|endoftext|>


<commit_before>#include <iostream>
#include <vector>

#include <boost/program_options.hpp>

#include "prob.h"
#include "tdict.h"
#include "ns.h"
#include "filelib.h"
#include "stringlib.h"

using namespace std;

namespace po = boost::program_options;

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("scale,a",po::value<double>()->default_value(1.0), "Posterior scaling factor (alpha)")
        ("evaluation_metric,m",po::value<string>()->default_value("ibm_bleu"), "Evaluation metric")
        ("input,i",po::value<string>()->default_value("-"), "File to read k-best lists from")
        ("output_list,L", "Show reranked list as output")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  bool flag = false;
  if (flag || conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

struct LossComparer {
  bool operator()(const pair<vector<WordID>, prob_t>& a, const pair<vector<WordID>, prob_t>& b) const {
    return a.second < b.second;
  }
};

bool ReadKBestList(istream* in, string* sent_id, vector<pair<vector<WordID>, prob_t> >* list) {
  static string cache_id;
  static pair<vector<WordID>, prob_t> cache_pair;
  list->clear();
  string cur_id;
  if (cache_pair.first.size() > 0) {
    list->push_back(cache_pair);
    cur_id = cache_id;
    cache_pair.first.clear();
  }
  string line;
  string tstr;
  while(*in) {
    getline(*in, line);
    if (line.empty()) continue;
    size_t p1 = line.find(" ||| ");
    if (p1 == string::npos) { cerr << "Bad format: " << line << endl; abort(); }
    size_t p2 = line.find(" ||| ", p1 + 4);
    if (p2 == string::npos) { cerr << "Bad format: " << line << endl; abort(); }
    size_t p3 = line.rfind(" ||| ");
    cache_id = line.substr(0, p1);
    tstr = line.substr(p1 + 5, p2 - p1 - 5);
    double val = strtod(line.substr(p3 + 5).c_str(), NULL);
    TD::ConvertSentence(tstr, &cache_pair.first);
    cache_pair.second.logeq(val);
    if (cur_id.empty()) cur_id = cache_id;
    if (cur_id == cache_id) {
      list->push_back(cache_pair);
      *sent_id = cur_id;
      cache_pair.first.clear();
    } else { break; }
  }
  return !list->empty();
}

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  const string smetric = conf["evaluation_metric"].as<string>();
  EvaluationMetric* metric = EvaluationMetric::Instance(smetric);

  const bool is_loss = (UppercaseString(smetric) == "TER");
  const bool output_list = conf.count("output_list") > 0;
  const string file = conf["input"].as<string>();
  const double mbr_scale = conf["scale"].as<double>();
  cerr << "Posterior scaling factor (alpha) = " << mbr_scale << endl;

  vector<pair<vector<WordID>, prob_t> > list;
  ReadFile rf(file);
  string sent_id;
  while(ReadKBestList(rf.stream(), &sent_id, &list)) {
    vector<prob_t> joints(list.size());
    const prob_t max_score = pow(list.front().second, mbr_scale);
    prob_t marginal = prob_t::Zero();
    for (int i = 0 ; i < list.size(); ++i) {
      const prob_t joint = pow(list[i].second, mbr_scale) / max_score;
      joints[i] = joint;
      // cerr << "list[" << i << "] joint=" << log(joint) << endl;
      marginal += joint;
    }
    int mbr_idx = -1;
    vector<double> mbr_scores(output_list ? list.size() : 0);
    double mbr_loss = numeric_limits<double>::max();
    for (int i = 0 ; i < list.size(); ++i) {
      const vector<vector<WordID> > refs(1, list[i].first);
      boost::shared_ptr<SegmentEvaluator> segeval = metric->
          CreateSegmentEvaluator(refs);

      double wl_acc = 0;
      for (int j = 0; j < list.size(); ++j) {
        if (i != j) {
          SufficientStats ss;
          segeval->Evaluate(list[j].first, &ss);
          double loss = 1.0 - metric->ComputeScore(ss);
          if (is_loss) loss = 1.0 - loss;
          double weighted_loss = loss * (joints[j] / marginal).as_float();
          wl_acc += weighted_loss;
          if ((!output_list) && wl_acc > mbr_loss) break;
        }
      }
      if (output_list) mbr_scores[i] = wl_acc;
      if (wl_acc < mbr_loss) {
        mbr_loss = wl_acc;
        mbr_idx = i;
      }
    }
    // cerr << "ML translation: " << TD::GetString(list[0].first) << endl;
    cerr << "MBR Best idx: " << mbr_idx << endl;
    if (output_list) {
      for (int i = 0; i < list.size(); ++i)
        list[i].second.logeq(mbr_scores[i]);
      sort(list.begin(), list.end(), LossComparer());
      for (int i = 0; i < list.size(); ++i)
        cout << sent_id << " ||| "
             << TD::GetString(list[i].first) << " ||| "
             << log(list[i].second) << endl;
    } else {
      cout << TD::GetString(list[mbr_idx].first) << endl;
    }
  }
  return 0;
}

<commit_msg>mbr fix for non-deduped lists<commit_after>#include <iostream>
#include <vector>
#include <tr1/unordered_map>

#include <boost/program_options.hpp>
#include <boost/functional/hash.hpp>

#include "prob.h"
#include "tdict.h"
#include "ns.h"
#include "filelib.h"
#include "stringlib.h"

using namespace std;
using namespace std::tr1;

namespace po = boost::program_options;

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("scale,a",po::value<double>()->default_value(1.0), "Posterior scaling factor (alpha)")
        ("evaluation_metric,m",po::value<string>()->default_value("ibm_bleu"), "Evaluation metric")
        ("input,i",po::value<string>()->default_value("-"), "File to read k-best lists from")
        ("output_list,L", "Show reranked list as output")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  bool flag = false;
  if (flag || conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

struct ScoreComparer {
  bool operator()(const pair<vector<WordID>, prob_t>& a, const pair<vector<WordID>, prob_t>& b) const {
    return a.second > b.second;
  }
};

struct LossComparer {
  bool operator()(const pair<vector<WordID>, prob_t>& a, const pair<vector<WordID>, prob_t>& b) const {
    return a.second < b.second;
  }
};

bool ReadKBestList(const double mbr_scale, istream* in, string* sent_id, vector<pair<vector<WordID>, prob_t> >* list) {
  static string cache_id;
  static pair<vector<WordID>, prob_t> cache_pair;
  list->clear();
  string cur_id;
  unordered_map<vector<WordID>, unsigned, boost::hash<vector<WordID> > > sent2id;
  if (cache_pair.first.size() > 0) {
    list->push_back(cache_pair);
    sent2id[cache_pair.first] = 0;
    cur_id = cache_id;
    cache_pair.first.clear();
  }
  string line;
  string tstr;
  while(getline(*in, line)) {
    size_t p1 = line.find(" ||| ");
    if (p1 == string::npos) { cerr << "Bad format: " << line << endl; abort(); }
    size_t p2 = line.find(" ||| ", p1 + 4);
    if (p2 == string::npos) { cerr << "Bad format: " << line << endl; abort(); }
    size_t p3 = line.rfind(" ||| ");
    cache_id = line.substr(0, p1);
    tstr = line.substr(p1 + 5, p2 - p1 - 5);
    double val = strtod(line.substr(p3 + 5).c_str(), NULL) * mbr_scale;
    TD::ConvertSentence(tstr, &cache_pair.first);
    cache_pair.second.logeq(val);
    if (cur_id.empty()) cur_id = cache_id;
    if (cur_id == cache_id) {
      unordered_map<vector<WordID>, unsigned, boost::hash<vector<WordID> > >::iterator it =
        sent2id.find(cache_pair.first);
      if (it == sent2id.end()) {
        sent2id.insert(make_pair(cache_pair.first, unsigned(list->size())));
        list->push_back(cache_pair);
      } else {
        (*list)[it->second].second += cache_pair.second;
        // cerr << "Cruch: " << line << "\n newp=" << (*list)[it->second].second << endl;
      }
      *sent_id = cur_id;
      cache_pair.first.clear();
    } else { break; }
  }
  sort(list->begin(), list->end(), ScoreComparer());
  return !list->empty();
}

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  const string smetric = conf["evaluation_metric"].as<string>();
  EvaluationMetric* metric = EvaluationMetric::Instance(smetric);

  const bool is_loss = (UppercaseString(smetric) == "TER");
  const bool output_list = conf.count("output_list") > 0;
  const string file = conf["input"].as<string>();
  const double mbr_scale = conf["scale"].as<double>();
  cerr << "Posterior scaling factor (alpha) = " << mbr_scale << endl;

  vector<pair<vector<WordID>, prob_t> > list;
  ReadFile rf(file);
  string sent_id;
  while(ReadKBestList(mbr_scale, rf.stream(), &sent_id, &list)) {
    vector<prob_t> joints(list.size());
    const prob_t max_score = list.front().second;
    prob_t marginal = prob_t::Zero();
    for (int i = 0 ; i < list.size(); ++i) {
      const prob_t joint = list[i].second / max_score;
      joints[i] = joint;
      //cerr << "list[" << i << "] joint=" << log(joint) << endl;
      marginal += joint;
    }
    int mbr_idx = -1;
    vector<double> mbr_scores(output_list ? list.size() : 0);
    double mbr_loss = numeric_limits<double>::max();
    for (int i = 0 ; i < list.size(); ++i) {
      const vector<vector<WordID> > refs(1, list[i].first);
      boost::shared_ptr<SegmentEvaluator> segeval = metric->
          CreateSegmentEvaluator(refs);

      double wl_acc = 0;
      for (int j = 0; j < list.size(); ++j) {
        if (i != j) {
          SufficientStats ss;
          segeval->Evaluate(list[j].first, &ss);
          double loss = 1.0 - metric->ComputeScore(ss);
          if (is_loss) loss = 1.0 - loss;
          double weighted_loss = loss * (joints[j] / marginal).as_float();
          wl_acc += weighted_loss;
          if ((!output_list) && wl_acc > mbr_loss) break;
        }
      }
      if (output_list) mbr_scores[i] = wl_acc;
      if (wl_acc < mbr_loss) {
        mbr_loss = wl_acc;
        mbr_idx = i;
      }
    }
    // cerr << "ML translation: " << TD::GetString(list[0].first) << endl;
    cerr << "MBR Best idx: " << mbr_idx << endl;
    if (output_list) {
      for (int i = 0; i < list.size(); ++i)
        list[i].second.logeq(mbr_scores[i]);
      sort(list.begin(), list.end(), LossComparer());
      for (int i = 0; i < list.size(); ++i)
        cout << sent_id << " ||| "
             << TD::GetString(list[i].first) << " ||| "
             << log(list[i].second) << endl;
    } else {
      cout << TD::GetString(list[mbr_idx].first) << endl;
    }
  }
  return 0;
}

<|endoftext|>